106 lines
4.9 KiB
C++
106 lines
4.9 KiB
C++
#include "Measurement.h"
|
|
#include "Config.h"
|
|
#include <string.h>
|
|
|
|
bool Measurement::start(float hz, float duty, float tolerance, uint32_t timeMs,
|
|
uint8_t repeats, uint8_t settleCycles) {
|
|
if (!task_ && xTaskCreate(taskEntry, "optical-rx", 4096, this, 4, &task_) != pdPASS) return false;
|
|
expectedHz_ = static_cast<uint32_t>(hz + 0.5f);
|
|
if (!expectedHz_ || !timeMs || !repeats || repeats > 10 ||
|
|
!makePeriodLimits(expectedHz_, duty, tolerance, receiver_.tickHz(), limits_) ||
|
|
!receiver_.start(expectedHz_)) return false;
|
|
timeMs_ = timeMs; repeats_ = repeats; settleLeft_ = settleCycles;
|
|
stats_.reset(); memset(repeatPeriods_, 0, sizeof(repeatPeriods_));
|
|
measurementStartTick_ = deadlineTick_ = 0; startedMs_ = millis();
|
|
measurementStartMs_ = lastPeriodMs_ = 0;
|
|
currentRepeat_ = 0;
|
|
expectedPeriodMs_ = static_cast<uint32_t>((1000ULL + expectedHz_ - 1U) / expectedHz_);
|
|
if (!expectedPeriodMs_) expectedPeriodMs_ = 1;
|
|
state_ = MeasureState::SETTLING;
|
|
xTaskNotifyGive(task_);
|
|
return true;
|
|
}
|
|
|
|
void Measurement::taskEntry(void *context) {
|
|
static_cast<Measurement *>(context)->taskLoop();
|
|
}
|
|
|
|
void Measurement::taskLoop() {
|
|
for (;;) {
|
|
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
|
while (state_ == MeasureState::SETTLING || state_ == MeasureState::RUNNING) processOnce();
|
|
}
|
|
}
|
|
|
|
void Measurement::fail(FailReason reason) {
|
|
if (stats_.reason == FailReason::NONE) stats_.reason = reason;
|
|
receiver_.stop(); state_ = MeasureState::FAIL;
|
|
}
|
|
|
|
void Measurement::completeWindow() {
|
|
receiver_.stop();
|
|
stats_.droppedItems += receiver_.takeDroppedItems();
|
|
if (receiver_.overflowed()) { fail(FailReason::GLITCH); return; }
|
|
for (uint8_t i = 0; i < repeats_; ++i) if (!repeatPeriods_[i]) {
|
|
fail(FailReason::TOO_FEW_PERIODS); return;
|
|
}
|
|
state_ = MeasureState::PASS;
|
|
}
|
|
|
|
MeasureState Measurement::processOnce() {
|
|
if (state_ != MeasureState::SETTLING && state_ != MeasureState::RUNNING) return state_;
|
|
if (receiver_.overflowed()) { fail(FailReason::GLITCH); return state_; }
|
|
bool receivedPeriod = false;
|
|
for (;;) {
|
|
const size_t periodCount = receiver_.readPeriods(periodBatch_, PERIOD_BATCH_SIZE, pdMS_TO_TICKS(2));
|
|
stats_.droppedItems += receiver_.takeDroppedItems();
|
|
if (!periodCount) break;
|
|
receivedPeriod = true;
|
|
for (size_t periodIndex = 0; periodIndex < periodCount; ++periodIndex) {
|
|
if (state_ != MeasureState::SETTLING && state_ != MeasureState::RUNNING) return state_;
|
|
const PulsePeriod &period = periodBatch_[periodIndex];
|
|
if (state_ == MeasureState::SETTLING) {
|
|
if (settleLeft_) --settleLeft_;
|
|
if (!settleLeft_) {
|
|
measurementStartTick_ = period.startTick + period.periodTicks;
|
|
repeatTicks_ = static_cast<uint64_t>(receiver_.tickHz()) * timeMs_ / 1000ULL;
|
|
deadlineTick_ = measurementStartTick_ + repeatTicks_ * repeats_;
|
|
nextRepeatTick_ = measurementStartTick_ + repeatTicks_;
|
|
stats_.reset(); measurementStartMs_ = lastPeriodMs_ = millis(); state_ = MeasureState::RUNNING;
|
|
}
|
|
continue;
|
|
}
|
|
const uint64_t endTick = period.startTick + period.periodTicks;
|
|
if (period.startTick < measurementStartTick_) continue; // leading incomplete period
|
|
if (endTick > deadlineTick_) { completeWindow(); return state_; } // trailing incomplete period
|
|
while (currentRepeat_ + 1U < repeats_ && period.startTick >= nextRepeatTick_) {
|
|
++currentRepeat_; nextRepeatTick_ += repeatTicks_;
|
|
}
|
|
++repeatPeriods_[currentRepeat_];
|
|
const FailReason r = evaluatePeriodFast(period, receiver_.tickHz(), limits_, currentRepeat_ + 1, stats_);
|
|
if (r != FailReason::NONE) { fail(r); return state_; }
|
|
}
|
|
}
|
|
if (receivedPeriod && state_ == MeasureState::RUNNING) lastPeriodMs_ = millis();
|
|
const uint64_t edgeBasedTimeout =
|
|
static_cast<uint64_t>(PWM_SETTLE_CYCLES + NO_SIGNAL_TIMEOUT_PERIODS) * expectedPeriodMs_ + 20;
|
|
const uint64_t rmtBatchTimeout =
|
|
static_cast<uint64_t>(RMT_MIN_RECEIVE_SYMBOLS + NO_SIGNAL_TIMEOUT_PERIODS) * expectedPeriodMs_ + 20;
|
|
const uint64_t settleTimeout = edgeBasedTimeout > rmtBatchTimeout ? edgeBasedTimeout : rmtBatchTimeout;
|
|
if (state_ == MeasureState::SETTLING && millis() - startedMs_ > settleTimeout) fail(FailReason::NO_SIGNAL);
|
|
if (state_ == MeasureState::RUNNING && measurementStartTick_) {
|
|
const uint32_t now = millis();
|
|
const uint32_t totalMs = timeMs_ * repeats_;
|
|
const uint32_t edgeTimeoutMs = expectedPeriodMs_ * NO_SIGNAL_TIMEOUT_PERIODS + 2;
|
|
if (now - measurementStartMs_ < totalMs && now - lastPeriodMs_ > edgeTimeoutMs) {
|
|
fail(FailReason::LOST_EDGE); return state_;
|
|
}
|
|
if (now - measurementStartMs_ > totalMs + expectedPeriodMs_ + 2) completeWindow();
|
|
}
|
|
return state_;
|
|
}
|
|
|
|
MeasureState Measurement::update() { return state_; }
|
|
|
|
void Measurement::abort() { if (state_ == MeasureState::SETTLING || state_ == MeasureState::RUNNING) fail(FailReason::ABORTED); }
|