diff --git a/lib/Espfc/src/Debug_Espfc.h b/lib/Espfc/src/Debug_Espfc.h index c536e46..15270be 100644 --- a/lib/Espfc/src/Debug_Espfc.h +++ b/lib/Espfc/src/Debug_Espfc.h @@ -28,9 +28,9 @@ template void D(T t) { if(!_debugStream) return; - _debugStream->print(t); - _debugStream->print('\r'); - _debugStream->print('\n'); + _debugStream->println(t); + //_debugStream->print('\r'); + //_debugStream->print('\n'); } template diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 89da77b..b96333a 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -66,9 +66,11 @@ class Espfc Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); _sensor.read(); - if(_model.state.inputTimer.check()) + if(_model.state.inputTimer.syncTo(_model.state.gyroTimer, 1u)) { + PIN_DEBUG(HIGH); _input.update(); + PIN_DEBUG(LOW); } if(_model.state.actuatorTimer.check()) { @@ -95,7 +97,12 @@ class Espfc _mixer.update(); } _blackbox.update(); - _input.update(); + if(_model.state.inputTimer.syncTo(_model.state.gyroTimer, 1u)) + { + PIN_DEBUG(HIGH); + _input.update(); + PIN_DEBUG(LOW); + } if(_model.state.actuatorTimer.check()) { _actuator.update(); diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 4a18ba5..0d7d6c2 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -429,11 +429,14 @@ class Model //state.accelTimer.setRate(constrain(state.gyroTimer.rate, 100, 500)); //state.accelTimer.setInterval(state.accelTimer.interval - 5); //state.accelTimer.setRate(state.gyroTimer.rate, 2); - int accelRate = Math::alignToClock(state.gyroRate, 500); + int accelRate = Math::alignToClock(state.gyroTimer.rate, 500); state.accelTimer.setRate(state.gyroTimer.rate, state.gyroTimer.rate / accelRate); state.loopTimer.setRate(state.gyroTimer.rate, config.loopSync); state.mixerTimer.setRate(state.loopTimer.rate, config.mixerSync); - state.inputTimer.setRate(1001); + //state.inputTimer.setRate(1005); + int inputRate = Math::alignToClock(state.gyroTimer.rate, 1000); + state.inputTimer.setRate(state.gyroTimer.rate, state.gyroTimer.rate / inputRate); + logger.info().log("INPUT RATE").log(state.gyroTimer.rate).log(inputRate).log(state.inputTimer.rate).log(state.inputTimer.interval).logln(state.inputTimer.denom); state.actuatorTimer.setRate(50); state.dynamicFilterTimer.setRate(50); state.telemetryTimer.setInterval(config.telemetryInterval * 1000); diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index c064606..5c446a0 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -146,13 +146,12 @@ class SerialManager port->begin(sdc); _model.state.serial[i].stream = port; - if(i == ESPFC_SERIAL_DEBUG_PORT) { initDebugStream(port); } - _model.logger.info().log(F("UART")).log(i).log(spc.id).log(spc.functionMask).log(sdc.baud).log(sdc.tx_pin).logln(sdc.rx_pin); + _model.logger.info().log(F("UART")).log(i).log(spc.id).log(spc.functionMask).log(sdc.baud).log(i == ESPFC_SERIAL_DEBUG_PORT).log(sdc.tx_pin).logln(sdc.rx_pin); } #ifdef ESPFC_SERIAL_SOFT_0_WIFI diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 4dabb6f..56695ed 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -154,9 +154,12 @@ class Stats float getCpuLoad() const { - float maxLoad = std::max(getLoad(COUNTER_CPU_0), getLoad(COUNTER_CPU_1)); - float minLoad = std::min(getLoad(COUNTER_CPU_0), getLoad(COUNTER_CPU_1)); - return 0.7f * maxLoad + 0.3f * minLoad; + float cpu0 = getLoad(COUNTER_CPU_0); + float cpu1 = getLoad(COUNTER_CPU_1); + float maxLoad = std::max(cpu0, cpu1); + float minLoad = std::min(cpu0, cpu1); + float alpha = maxLoad / (minLoad + maxLoad); + return alpha * maxLoad + (1.f - alpha) * minLoad; } float getCpuTime() const diff --git a/lib/Espfc/src/Timer.cpp b/lib/Espfc/src/Timer.cpp index 0c10d40..07e187b 100644 --- a/lib/Espfc/src/Timer.cpp +++ b/lib/Espfc/src/Timer.cpp @@ -1,5 +1,5 @@ -#include "Timer.h" #include +#include "Timer.h" #include "Utils/MemoryHelper.h" namespace Espfc { @@ -10,56 +10,61 @@ Timer::Timer(): interval(0), last(0), next(0), iteration(0), delta(0) int Timer::setInterval(uint32_t interval) { - this->interval = interval; - this->rate = 1000000UL / interval; - this->denom = 1; - this->delta = this->interval; - this->intervalf = this->interval * 0.000001f; - iteration = 0; - return 1; + this->interval = interval; + this->rate = 1000000UL / interval; + this->denom = 1; + this->delta = this->interval; + this->intervalf = this->interval * 0.000001f; + iteration = 0; + return 1; } int Timer::setRate(uint32_t rate, uint32_t denom) { - this->rate = rate / denom; - this->interval = 1000000UL / this->rate; - this->denom = denom; - this->delta = this->interval; - this->intervalf = this->interval * 0.000001f; - iteration = 0; - return 1; + this->rate = rate / denom; + this->interval = 1000000UL / this->rate; + this->denom = denom; + this->delta = this->interval; + this->intervalf = this->interval * 0.000001f; + iteration = 0; + return 1; } bool FAST_CODE_ATTR Timer::check() { - return check(micros()); + return check(micros()); } int FAST_CODE_ATTR Timer::update() { - return update(micros()); + return update(micros()); } bool FAST_CODE_ATTR Timer::check(uint32_t now) { - if(interval == 0) return false; - if(now < next) return false; - return update(now); + if(interval == 0) return false; + if(now < next) return false; + return update(now); } int FAST_CODE_ATTR Timer::update(uint32_t now) { - next = now + interval; - delta = now - last; - last = now; - iteration++; - return 1; + next = now + interval; + delta = now - last; + last = now; + iteration++; + return 1; } -bool FAST_CODE_ATTR Timer::syncTo(const Timer& t) +bool FAST_CODE_ATTR Timer::syncTo(const Timer& t, uint32_t slot) { - if(t.iteration % denom != 0) return false; - return update(); + if(denom > 0) + { + if(slot > denom - 1) slot = denom - 1; + if(t.iteration % denom != slot) return false; + return update(micros()); + } + return check(micros()); } } diff --git a/lib/Espfc/src/Timer.h b/lib/Espfc/src/Timer.h index 9305475..f2ba25f 100644 --- a/lib/Espfc/src/Timer.h +++ b/lib/Espfc/src/Timer.h @@ -9,13 +9,13 @@ class Timer public: Timer(); int setInterval(uint32_t interval); - int setRate(uint32_t rate, uint32_t denom = 1); + int setRate(uint32_t rate, uint32_t denom = 1u); bool check(); int update(); bool check(uint32_t now); int update(uint32_t now); - bool syncTo(const Timer& t); + bool syncTo(const Timer& t, uint32_t slot = 0u); uint32_t interval; uint32_t rate;