Timer::syncTo() slot arg + shift input processing

rtlopez · Apr 24, 2024 · b0374ad · b0374ad
1 parent f546296
commit b0374ad
Show file tree

Hide file tree

Showing 7 changed files with 59 additions and 42 deletions.
diff --git a/lib/Espfc/src/Debug_Espfc.h b/lib/Espfc/src/Debug_Espfc.h
@@ -28,9 +28,9 @@ template <typename T>
 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<typename T, typename... Args>

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -1,5 +1,5 @@
-#include "Timer.h"
 #include <Arduino.h>
+#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
@@ -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;