Merge pull request #102 from rtlopez/atomic-queue

Processing optimization
rtlopez · Dec 11, 2023 · 8c731d3 · 8c731d3
2 parents 6f3ea52 + 75677b2
commit 8c731d3
Show file tree

Hide file tree

Showing 20 changed files with 296 additions and 150 deletions.
diff --git a/docs/setup.md b/docs/setup.md
@@ -126,9 +126,10 @@ Logging using serial device is possible, like [D-ronin OpenLager](https://github
 Recommended settings
 
 - To log with 1k rate, you need device that is capable to receive data with 250kbps.
-- To log with 2k rate, you need 500kbps
+- To log with 1k rate with debug, baro and mag data, 500kbps is recommended.
+- To log with 2k rate, you need 1Mbps
 
-OpenLager can handle it easily. If you plan to use OpenLog, you might need to flash [blackbox-firmware](https://github.com/cleanflight/blackbox-firmware) to be able to handle more than 115.2kbps. Either 250kbps and 500kbps works well with modern SD cards up to 32GB.
+OpenLager can handle it easily. If you plan to use OpenLog, you might need to flash [blackbox-firmware](https://github.com/cleanflight/blackbox-firmware) to be able to handle more than 115.2kbps. Either 250kbps and 500kbps works well with modern SD cards up to 32GB size.
 
 ## Limitations
 

diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
@@ -35,15 +35,17 @@ class BlackboxBuffer
 
     size_t availableForWrite() const
     {
+      //return _stream->availableForWrite();
       return SIZE - _idx;
     }
 
     size_t isTxFifoEmpty() const
     {
+      //return _stream->isTxFifoEmpty();
       return _idx == 0;
     }
 
-    static const size_t SIZE = 128;
+    static const size_t SIZE = SERIAL_TX_FIFO_SIZE;//128;
 
     Espfc::Device::SerialDevice * _stream;
     size_t _idx;
@@ -317,20 +319,6 @@ class Blackbox
       return 1;
     }
 
-    int onAppEvent(const Event& e)
-    {
-      switch(e.type)
-      {
-        case EVENT_MIXER_UPDATED:
-          update();
-          _model.state.appQueue.send(Event(EVENT_BBLOG_UPDATED));
-          return 1;
-        default:
-          break;
-      }
-      return 0;
-    }
-
     int update()
     {
       if(!_model.blackboxEnabled()) return 0;

diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h
@@ -19,27 +19,6 @@ class Controller
       return 1;
     }
 
-    int onAppEvent(const Event& e)
-    {
-      switch(e.type)
-      {
-        case EVENT_GYRO_READ:
-          _model.state.loopUpdate = true;
-          return 1;
-        case EVENT_SENSOR_READ:
-          if(_model.state.loopUpdate)
-          {
-            update();
-            _model.state.loopUpdate = false;
-            _model.state.appQueue.send(Event(EVENT_PID_UPDATED));
-          }
-          return 1;
-        default:
-          break;
-      }
-      return 0;
-    }
-
     int update()
     {
       uint32_t startTime = 0;

diff --git a/lib/Espfc/src/Device/InputCRSF.h b/lib/Espfc/src/Device/InputCRSF.h
@@ -44,9 +44,16 @@ class InputCRSF: public InputDevice
       if(!_serial) return INPUT_IDLE;
 
       size_t len = _serial->available();
-      while(len--)
+      if(len)
       {
-        parse(_frame, _serial->read());
+        uint8_t buff[64] = {0};
+        len = std::min(len, sizeof(buff));
+        _serial->readMany(buff, len);
+        size_t i = 0;
+        while(i < len)
+        {
+          parse(_frame, buff[i++]);
+        }
       }
 
       if(_new_data)

diff --git a/lib/Espfc/src/Device/InputSBUS.h b/lib/Espfc/src/Device/InputSBUS.h
@@ -68,9 +68,16 @@ class InputSBUS: public InputDevice
       if(!_serial) return INPUT_IDLE;
 
       size_t len = _serial->available();
-      while(len--)
+      if(len)
       {
-        parse(_serial->read());
+        uint8_t buff[64] = {0};
+        len = std::min(len, sizeof(buff));
+        _serial->readMany(buff, len);
+        size_t i = 0;
+        while(i < len)
+        {
+          parse(buff[i++]);
+        }
       }
 
       if(_new_data)

diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h
@@ -66,7 +66,10 @@ class Espfc
       Stats::Measure measure(_model.state.stats, COUNTER_CPU_0);
 
       _sensor.read();
-      _input.update();
+      if(_model.state.inputTimer.check())
+      {
+        _input.update();
+      }
       if(_model.state.actuatorTimer.check())
       {
         _actuator.update();
@@ -91,7 +94,10 @@ class Espfc
           {
             _mixer.update();
           }
-          _input.update();
+          if(_model.state.inputTimer.check())
+          {
+            _input.update();
+          }
           if(_model.state.actuatorTimer.check())
           {
             _actuator.update();
@@ -109,14 +115,30 @@ class Espfc
     int updateOther()
     {
 #if defined(ESPFC_MULTI_CORE)
+      if(_model.state.appQueue.isEmpty())
+      {
+        return 0;
+      }
       Event e = _model.state.appQueue.receive();
 
       Stats::Measure measure(_model.state.stats, COUNTER_CPU_1);
 
-      _sensor.onAppEvent(e);
-      _controller.onAppEvent(e);
-      _mixer.onAppEvent(e);
-      _blackbox.onAppEvent(e);
+      switch(e.type)
+      {
+        case EVENT_GYRO_READ:
+          _sensor.preLoop();
+          _controller.update();
+          _mixer.update();
+          _blackbox.update();
+          _sensor.postLoop();
+          break;
+        case EVENT_ACCEL_READ:
+          _sensor.fusion();
+          break;
+        default:
+          break;
+          // nothing
+      }
 #else
       if(_model.state.serialTimer.check())
       {

diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
@@ -434,11 +434,12 @@ class Model
       //state.accelTimer.setRate(state.gyroTimer.rate, 2);
       state.loopTimer.setRate(state.gyroTimer.rate, config.loopSync);
       state.mixerTimer.setRate(state.loopTimer.rate, config.mixerSync);
-      state.actuatorTimer.setRate(25); // 25 hz
+      state.inputTimer.setRate(1000);
+      state.actuatorTimer.setRate(50);
       state.dynamicFilterTimer.setRate(50);
       state.telemetryTimer.setInterval(config.telemetryInterval * 1000);
       state.stats.timer.setRate(2);
-      state.serialTimer.setRate(1000);
+      state.serialTimer.setRate(4000);
       if(magActive())
       {
         state.magTimer.setRate(state.magRate);
@@ -466,8 +467,8 @@ class Model
         } else {
           state.gyroFilter[i].begin(config.gyroFilter, gyroFilterRate);
         }
-        state.gyroFilter2[i].begin(config.gyroFilter2, gyroPreFilterRate);
-        state.gyroFilter3[i].begin(config.gyroFilter3, gyroFilterRate);
+        state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate);
+        state.gyroFilter3[i].begin(config.gyroFilter3, gyroPreFilterRate);
         state.accelFilter[i].begin(config.accelFilter, gyroFilterRate);
         state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 2), gyroFilterRate);
         if(magActive())

diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
@@ -252,6 +252,7 @@ struct ModelState
   float maxThrottle;
   bool digitalOutput;
 
+  Timer inputTimer;
   Timer actuatorTimer;
 
   Timer magTimer;

diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h
@@ -57,22 +57,6 @@ class Mixer
       return 1;
     }
 
-    int onAppEvent(const Event& e)
-    {
-      switch(e.type)
-      {
-        case EVENT_PID_UPDATED:
-          if(_model.state.mixerTimer.syncTo(_model.state.loopTimer)) {
-            update();
-          }
-          _model.state.appQueue.send(Event(EVENT_MIXER_UPDATED));
-          return 1;
-        default:
-          break;
-      }
-      return 0;
-    }
-
     int update()
     {
       uint32_t startTime = micros();

diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -73,13 +73,15 @@ class GyroSensor: public BaseSensor
 
       VectorFloat input = (VectorFloat)_model.state.gyroRaw * _model.state.gyroScale;
 
-      if(_model.config.gyroFilter2.freq)
+      if(_model.config.gyroFilter3.freq)
       {
         for(size_t i = 0; i < 3; ++i)
         {
-          _model.state.gyroSampled.set(i, _model.state.gyroFilter2[i].update(input[i]));
+          _model.state.gyroSampled.set(i, _model.state.gyroFilter3[i].update(input[i]));
         }
-      } else {
+      }
+      else
+      {
         // moving average filter
         _model.state.gyroSampled = _sma.update(input);
       }
@@ -117,7 +119,7 @@ class GyroSensor: public BaseSensor
           _model.state.debug[0] = lrintf(degrees(_model.state.gyro[i]));
         }
 
-        _model.state.gyro.set(i, _model.state.gyroFilter3[i].update(_model.state.gyro[i]));
+        _model.state.gyro.set(i, _model.state.gyroFilter2[i].update(_model.state.gyro[i]));
 
         if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis)
         {

diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h
@@ -31,50 +31,15 @@ class SensorManager
       return 1;
     }
 
-    int onAppEvent(const Event& e)
-    {
-      switch(e.type)
-      {
-        case EVENT_GYRO_READ:
-          _gyro.filter();
-          if(_model.state.gyroBiasSamples == 0)
-          {
-            _model.state.gyroBiasSamples = -1;
-            _fusion.restoreGain();
-          }
-          return 1;
-        case EVENT_ACCEL_READ:
-          _accel.filter();
-          _model.state.imuUpdate = true;
-          return 1;
-        case EVENT_MAG_READ:
-          return 1;
-        case EVENT_BBLOG_UPDATED:
-          _gyro.dynNotchAnalyze();
-          if(_model.state.imuUpdate)
-          {
-            _fusion.update();
-            _model.state.imuUpdate = false;
-            _model.state.appQueue.send(Event(EVENT_IMU_UPDATED));
-          }
-          return 1;
-        default:
-          break;
-      }
-      return 0;
-    }
-
     int read()
     {
-      _model.state.appQueue.send(Event(EVENT_START));
-
       _gyro.read();
       if(_model.state.loopTimer.syncTo(_model.state.gyroTimer))
       {
         _model.state.appQueue.send(Event(EVENT_GYRO_READ));
       }
 
-      int status = _accel.read();
+      int status = _accel.update();
       if(status)
       {
         _model.state.appQueue.send(Event(EVENT_ACCEL_READ));
@@ -84,34 +49,42 @@ class SensorManager
       {
         status = _mag.update();
       }
-      if(status)
-      {
-        _model.state.appQueue.send(Event(EVENT_MAG_READ));
-      }
 
       if(!status)
       {
         status = _baro.update();
       }
-      if(status)
-      {
-        _model.state.appQueue.send(Event(EVENT_BARO_READ));
-      }
 
       if(!status)
       {
         status = _voltage.update();
       }
-      if(status)
+
+      return 1;
+    }
+
+    int preLoop()
+    {
+      _gyro.filter();
+      if(_model.state.gyroBiasSamples == 0)
       {
-        _model.state.appQueue.send(Event(EVENT_VOLTAGE_READ));
+        _model.state.gyroBiasSamples = -1;
+        _fusion.restoreGain();
       }
+      return 1;
+    }
 
-      _model.state.appQueue.send(Event(EVENT_SENSOR_READ));
-
+    int postLoop()
+    {
+      _gyro.dynNotchAnalyze();
       return 1;
     }
 
+    int fusion()
+    {
+      return _fusion.update();
+    }
+
     // main task
     int update()
     {

diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h
@@ -146,10 +146,7 @@ class Stats
 
     float getCpuLoad() const
     {
-      float load = getLoad(COUNTER_CPU_0) + getLoad(COUNTER_CPU_1);
-#ifdef ESPFC_MULTI_CORE
-      load *= 0.5f;
-#endif
+      float load = std::max(getLoad(COUNTER_CPU_0), getLoad(COUNTER_CPU_1));
       return load;
     }