From de8c6deb5e5a08448fbb2d624789d86a708144a3 Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Wed, 23 Aug 2023 10:50:45 +0200
Subject: [PATCH 1/7] serial and vbat read optimizations

---
 lib/EspSoftSerial/src/EspSoftSerial.h      |  10 ++
 lib/Espfc/src/Device/SerialDevice.h        |   1 +
 lib/Espfc/src/Device/SerialDeviceAdapter.h |  12 ++
 lib/Espfc/src/Espfc.h                      |   5 -
 lib/Espfc/src/Msp/MspProcessor.h           |  13 ++-
 lib/Espfc/src/Sensor/VoltageSensor.h       | 128 ++++++++++++---------
 lib/Espfc/src/SensorManager.h              |   2 +-
 lib/Espfc/src/SerialManager.h              |  21 ++--
 lib/Espfc/src/Timer.h                      |   8 +-
 9 files changed, 125 insertions(+), 75 deletions(-)

diff --git a/lib/EspSoftSerial/src/EspSoftSerial.h b/lib/EspSoftSerial/src/EspSoftSerial.h
index 504a9f3e..da4d96fc 100644
--- a/lib/EspSoftSerial/src/EspSoftSerial.h
+++ b/lib/EspSoftSerial/src/EspSoftSerial.h
@@ -49,6 +49,16 @@ class EspSoftSerial: public Stream
       return _rx_buff.get();
     }
 
+    int read(uint8_t * buff, size_t len) override
+    {
+      size_t count = std::min(len, (size_t)available());
+      for(size_t i = 0; i < count; i++)
+      {
+        buff[i] = read();
+      }
+      return count;
+    }
+
     int peek() override
     {
       return _rx_buff.peek();
diff --git a/lib/Espfc/src/Device/SerialDevice.h b/lib/Espfc/src/Device/SerialDevice.h
index 9cd5c6dc..d5c9ce84 100644
--- a/lib/Espfc/src/Device/SerialDevice.h
+++ b/lib/Espfc/src/Device/SerialDevice.h
@@ -133,6 +133,7 @@ class SerialDevice: public Stream
     virtual void begin(const SerialDeviceConfig& conf) = 0;
     virtual int available() = 0;
     virtual int read() = 0;
+    virtual size_t readMany(uint8_t * c, size_t l) = 0;
     virtual int peek() = 0;
     virtual void flush() = 0;
     virtual size_t write(uint8_t c) = 0;
diff --git a/lib/Espfc/src/Device/SerialDeviceAdapter.h b/lib/Espfc/src/Device/SerialDeviceAdapter.h
index 90b6a444..4ada1324 100644
--- a/lib/Espfc/src/Device/SerialDeviceAdapter.h
+++ b/lib/Espfc/src/Device/SerialDeviceAdapter.h
@@ -21,6 +21,18 @@ class SerialDeviceAdapter: public SerialDevice
     virtual void begin(const SerialDeviceConfig& conf);
     virtual int available() { return _dev.available(); }
     virtual int read() { return _dev.read(); }
+    virtual size_t readMany(uint8_t * c, size_t l) {
+#ifdef TARGET_RP2040
+      size_t count = std::min(l, (size_t)available());
+      for(size_t i = 0; i < count; i++)
+      {
+        c[i] = read();
+      }
+      return count;
+#else
+      return _dev.read(c, l);
+#endif
+    }
     virtual int peek() { return _dev.peek(); }
     virtual void flush() { _dev.flush(); }
     virtual size_t write(uint8_t c) { return _dev.write(c); }
diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h
index 49f60990..c5f46fd0 100644
--- a/lib/Espfc/src/Espfc.h
+++ b/lib/Espfc/src/Espfc.h
@@ -59,11 +59,6 @@ class Espfc
     int update()
     {
 #if defined(ESPFC_MULTI_CORE)
-      /*if(!_model.state.appQueue.isEmpty())
-      {
-        return 0;
-      }*/
-
       if(!_model.state.gyroTimer.check())
       {
         return 0;
diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h
index ab5a48f3..43f10ba2 100644
--- a/lib/Espfc/src/Msp/MspProcessor.h
+++ b/lib/Espfc/src/Msp/MspProcessor.h
@@ -155,7 +155,7 @@ class MspProcessor
   public:
     MspProcessor(Model& model): _model(model) {}
 
-    bool process(char c, MspMessage& msg, MspResponse& res, Stream& s)
+    bool process(char c, MspMessage& msg, MspResponse& res, Device::SerialDevice& s)
     {
       _parser.parse(c, msg);
 
@@ -1377,7 +1377,7 @@ class MspProcessor
       }
     }
 
-    void sendResponse(MspResponse& r, Stream& s)
+    void sendResponse(MspResponse& r, Device::SerialDevice& s)
     {
       debugResponse(r);
       switch(r.version)
@@ -1392,8 +1392,9 @@ class MspProcessor
       postCommand();
     }
 
-    void sendResponseV1(MspResponse& r, Stream& s)
+    void sendResponseV1(MspResponse& r, Device::SerialDevice& s)
     {
+      // TODO: optimize to write at once
       uint8_t hdr[5] = { '$', 'M', '>' };
       if(r.result == -1)
       {
@@ -1411,7 +1412,7 @@ class MspProcessor
       s.write(checksum);
     }
 
-    void sendResponseV2(MspResponse& r, Stream& s)
+    void sendResponseV2(MspResponse& r, Device::SerialDevice& s)
     {
       uint8_t hdr[8] = { '$', 'X', '>', 0 };
       if(r.result == -1)
@@ -1463,7 +1464,7 @@ class MspProcessor
     void debugMessage(const MspMessage& m)
     {
       if(debugSkip(m.cmd)) return;
-      Stream * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT);
+      Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT);
       if(!s) return;
 
       s->print(m.dir == MSP_TYPE_REPLY ? '>' : '<'); s->print(' ');
@@ -1479,7 +1480,7 @@ class MspProcessor
     void debugResponse(const MspResponse& r)
     {
       if(debugSkip(r.cmd)) return;
-      Stream * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT);
+      Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT);
       if(!s) return;
 
       s->print(r.result == 1 ? '>' : (r.result == -1 ? '!' : '@')); s->print(' ');
diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h
index 66ea1f2c..847aeaff 100644
--- a/lib/Espfc/src/Sensor/VoltageSensor.h
+++ b/lib/Espfc/src/Sensor/VoltageSensor.h
@@ -12,19 +12,25 @@ namespace Sensor {
 class VoltageSensor: public BaseSensor
 {
   public:
+    enum State {
+      VBAT,
+      IBAT
+    };
     VoltageSensor(Model& model): _model(model) {}
 
     int begin()
     {
-      _model.state.battery.timer.setRate(50);
+      _model.state.battery.timer.setRate(100);
       _model.state.battery.samples = 50;
 
-      _vFilterFast.begin(FilterConfig(FILTER_PT1, 15), _model.state.battery.timer.rate);
+      _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate);
       _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate);
 
-      _iFilterFast.begin(FilterConfig(FILTER_PT1, 15), _model.state.battery.timer.rate);
+      _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate);
       _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate);
 
+      _state = VBAT;
+
       return 1;
     }
 
@@ -34,66 +40,81 @@ class VoltageSensor: public BaseSensor
 
       Stats::Measure measure(_model.state.stats, COUNTER_BATTERY);
 
-      int result = 0;
+      switch(_state) {
+        case VBAT:
+          _state = IBAT;
+          return readVbat();
+        case IBAT:
+          _state = VBAT;
+          return readIbat();
+      }
 
+      return 0;
+    }
+
+    int readVbat()
+    {
 #ifdef ESPFC_ADC_0
-      if(_model.config.vbatSource == 1 && _model.config.pin[PIN_INPUT_ADC_0] != -1)
+      if(_model.config.vbatSource != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0;
+      // wemos d1 mini has divider 3.2:1 (220k:100k)
+      // additionaly I've used divider 5.7:1 (4k7:1k)
+      // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal,
+      // but ~52:1 is real, did I miss something?
+      _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]);
+      float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE);
+
+      volts *= _model.config.vbatScale * 0.1f;
+      volts *= _model.config.vbatResMult;
+      volts /= _model.config.vbatResDiv;
+
+      _model.state.battery.voltageUnfiltered = volts;
+      _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered);
+
+      // cell count detection
+      if(_model.state.battery.samples > 0)
       {
-        // wemos d1 mini has divider 3.2:1 (220k:100k)
-        // additionaly I've used divider 5.7:1 (4k7:1k)
-        // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal,
-        // but ~52:1 is real, did I miss something?
-        _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]);
-        float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE);
-
-        volts *= _model.config.vbatScale * 0.1f;
-        volts *= _model.config.vbatResMult;
-        volts /= _model.config.vbatResDiv;
-
-        _model.state.battery.voltageUnfiltered = volts;
-        _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered);
-
-        // cell count detection
-        if(_model.state.battery.samples > 0)
-        {
-          _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f);
-          _model.state.battery.samples--;
-        }
-        _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6);
-
-        if(_model.config.debugMode == DEBUG_BATTERY)
-        {
-          _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000);
-          _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000);
-        }
-        result = 1;
+        _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f);
+        _model.state.battery.samples--;
       }
+      _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6);
+
+      if(_model.config.debugMode == DEBUG_BATTERY)
+      {
+        _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000);
+        _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000);
+      }
+      return 1;
+#else
+      return 0;
 #endif
+    }
 
+    int readIbat()
+    {
 #ifdef ESPFC_ADC_1
-      if(_model.config.ibatSource == 1 && _model.config.pin[PIN_INPUT_ADC_1] != -1)
+      if(_model.config.ibatSource != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0;
+
+      _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]);
+      float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE);
+      float milivolts = volts * 1000.0f;
+
+      volts += _model.config.ibatOffset * 0.001f;
+      volts *= _model.config.ibatScale * 0.1f;
+
+      _model.state.battery.currentUnfiltered = volts;
+      _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered);
+
+      if(_model.config.debugMode == DEBUG_CURRENT_SENSOR)
       {
-        _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]);
-        float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE);
-        float milivolts = volts * 1000.0f;
-
-        volts += _model.config.ibatOffset * 0.001f;
-        volts *= _model.config.ibatScale * 0.1f;
-
-        _model.state.battery.currentUnfiltered = volts;
-        _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered);
-
-        if(_model.config.debugMode == DEBUG_CURRENT_SENSOR)
-        {
-          _model.state.debug[0] = lrintf(milivolts);
-          _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000);
-          _model.state.debug[2] = _model.state.battery.rawCurrent;
-        }
-        result = 1;
+        _model.state.debug[0] = lrintf(milivolts);
+        _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000);
+        _model.state.debug[2] = _model.state.battery.rawCurrent;
       }
-#endif
 
-      return result;
+      return 1;
+#else
+      return 0;
+#endif
     }
 
   private:
@@ -102,6 +123,7 @@ class VoltageSensor: public BaseSensor
     Filter _vFilter;
     Filter _iFilterFast;
     Filter _iFilter;
+    State _state;
 };
 
 }
diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h
index e20c9fca..99839bd3 100644
--- a/lib/Espfc/src/SensorManager.h
+++ b/lib/Espfc/src/SensorManager.h
@@ -102,7 +102,7 @@ class SensorManager
       {
         status = _voltage.update();
       }
-      if (status)
+      if(status)
       {
         _model.state.appQueue.send(Event(EVENT_VOLTAGE_READ));
       }
diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h
index 39d51483..64886ea3 100644
--- a/lib/Espfc/src/SerialManager.h
+++ b/lib/Espfc/src/SerialManager.h
@@ -158,7 +158,7 @@ class SerialManager
       //D("serial", _current);
       SerialPortState& ss = _model.state.serial[_current];
       const SerialPortConfig& sc = _model.config.serial[_current];
-      Stream * stream = ss.stream;
+      Device::SerialDevice * stream = ss.stream;
 
       {
         Stats::Measure measure(_model.state.stats, COUNTER_SERIAL);
@@ -168,16 +168,23 @@ class SerialManager
         }
 
         size_t len = stream->available();
-        while(len--)
+        if(len > 0)
         {
-          const int c = stream->read();
-          if(sc.functionMask & SERIAL_FUNCTION_MSP)
+          uint8_t buff[64] = {0};
+          len = std::min(len, sizeof(buff));
+          stream->readMany(buff, len);
+          char * c = (char*)&buff[0];
+          while(len--)
           {
-            bool consumed = _msp.process(c, ss.mspRequest, ss.mspResponse, *stream);
-            if(!consumed)
+            if(sc.functionMask & SERIAL_FUNCTION_MSP)
             {
-              _cli.process(c, ss.cliCmd, *stream);
+              bool consumed = _msp.process(*c, ss.mspRequest, ss.mspResponse, *stream);
+              if(!consumed)
+              {
+                _cli.process(*c, ss.cliCmd, *stream);
+              }
             }
+            c++;
           }
         }
       }
diff --git a/lib/Espfc/src/Timer.h b/lib/Espfc/src/Timer.h
index 82e2dd29..47b109c4 100644
--- a/lib/Espfc/src/Timer.h
+++ b/lib/Espfc/src/Timer.h
@@ -9,7 +9,7 @@ namespace Espfc {
 class Timer
 {
   public:
-    Timer(): interval(0), last(0), iteration(0), delta(0) {}
+    Timer(): interval(0), last(0), next(0), iteration(0), delta(0) {}
 
     int setInterval(uint32_t interval)
     {
@@ -46,15 +46,16 @@ class Timer
     bool check(uint32_t now)
     {
       if(interval == 0) return false;
-      if(last + interval > now) return false;
+      if(now < next) return false;
       return update(now);
     }
 
     int update(uint32_t now)
     {
+      next = now + interval;
       delta = now - last;
-      iteration++;
       last = now;
+      iteration++;
       return 1;
     }
 
@@ -69,6 +70,7 @@ class Timer
     uint32_t denom;
 
     uint32_t last;
+    uint32_t next;
     uint32_t iteration;
     uint32_t delta;
     float intervalf;

From c815f07dce8787f7e1bb3b2a476572750cf8226c Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Thu, 24 Aug 2023 00:04:04 +0200
Subject: [PATCH 2/7] esp32 dshot tuning + imu after loop

---
 lib/EscDriver/src/EscDriverEsp32.h | 22 +++++++++++-----------
 lib/Espfc/src/Blackbox.h           |  2 +-
 lib/Espfc/src/Controller.h         |  2 +-
 lib/Espfc/src/SensorManager.h      |  4 ++--
 4 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h
index 6f085335..bde8e2b7 100644
--- a/lib/EscDriver/src/EscDriverEsp32.h
+++ b/lib/EscDriver/src/EscDriverEsp32.h
@@ -8,7 +8,7 @@
 #include <driver/rmt.h>
 
 //#define DURATION  12.5 /* flash 80MHz => minimum time unit in ns */
-static const size_t DURATION_CLOCK = 25; // [ns] duobled value to increase precision
+static const size_t DURATION_CLOCK = 25; // [ns] doubled value to increase precision
 static const size_t ITEM_COUNT = EscDriverBase::DSHOT_BIT_COUNT + 1;
 static const int32_t DURATION_MAX = 0x7fff; // max in 15 bits
 
@@ -80,8 +80,8 @@ class EscDriverEsp32: public EscDriverBase
 
         void setDshotBit(int item, bool val)
         {
-          const uint32_t th = (val ? dshot_t1h : dshot_t0h) & 0x7fff;
-          const uint32_t tl = (val ? dshot_t1l : dshot_t0l) & 0x7fff;
+          const uint32_t th = (val ? dshot_t1h : dshot_t0h) & DURATION_MAX;
+          const uint32_t tl = (val ? dshot_t1l : dshot_t0l) & DURATION_MAX;
           items[item].val = th | 1 << 15 | tl << 16;
           //items[item].duration0 = th;
           //items[item].level0 = 1;
@@ -186,10 +186,10 @@ class EscDriverEsp32: public EscDriverBase
       //_channel[i].dshot_t1l = getDshotPulse(420);
 
       // betaflight 0:35%, 1:70% of 1670ns for dshot600
-      _channel[i].dshot_t0h = getDshotPulse(584 - 2);
-      _channel[i].dshot_t0l = getDshotPulse(1086 - 2);
-      _channel[i].dshot_t1h = getDshotPulse(1170 - 2);
-      _channel[i].dshot_t1l = getDshotPulse(500 - 2);
+      _channel[i].dshot_t0h = getDshotPulse(584 - 16);
+      _channel[i].dshot_t0l = getDshotPulse(1086 - 32);
+      _channel[i].dshot_t1h = getDshotPulse(1170 - 32);
+      _channel[i].dshot_t1l = getDshotPulse(500 - 16);
 
       _channel[i].dev.gpio_num = pin;
       _channel[i].dev.rmt_mode = RMT_MODE_TX;
@@ -374,14 +374,14 @@ class EscDriverEsp32: public EscDriverBase
       return ns / ((div * DURATION_CLOCK) / 2);
     }
 
-    uint16_t getDshotPulse(uint16_t width) const
+    uint16_t getDshotPulse(uint32_t width) const
     {
       int div = getClockDivider();
       switch(_protocol)
       {
-        case ESC_PROTOCOL_DSHOT150:  return (width / (div * DURATION_CLOCK / 2)) * 4;
-        case ESC_PROTOCOL_DSHOT300:  return (width / (div * DURATION_CLOCK / 2)) * 2;
-        case ESC_PROTOCOL_DSHOT600:  return (width / (div * DURATION_CLOCK / 2)) * 1;
+        case ESC_PROTOCOL_DSHOT150:  return ((width * 4) / (div * DURATION_CLOCK / 2));
+        case ESC_PROTOCOL_DSHOT300:  return ((width * 2) / (div * DURATION_CLOCK / 2));
+        case ESC_PROTOCOL_DSHOT600:  return ((width * 1) / (div * DURATION_CLOCK / 2));
         case ESC_PROTOCOL_BRUSHED:
         case ESC_PROTOCOL_PWM:
         case ESC_PROTOCOL_ONESHOT125:
diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
index f22bbd52..93831fcc 100644
--- a/lib/Espfc/src/Blackbox.h
+++ b/lib/Espfc/src/Blackbox.h
@@ -323,7 +323,7 @@ class Blackbox
       {
         case EVENT_MIXER_UPDATED:
           update();
-          //_model.state.appQueue.send(Event(EVENT_BBLOG_UPDATED));
+          _model.state.appQueue.send(Event(EVENT_BBLOG_UPDATED));
           return 1;
         default:
           break;
diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h
index 3fe2f4b9..49bf6209 100644
--- a/lib/Espfc/src/Controller.h
+++ b/lib/Espfc/src/Controller.h
@@ -26,7 +26,7 @@ class Controller
         case EVENT_GYRO_READ:
           _model.state.loopUpdate = true;
           return 1;
-        case EVENT_IMU_UPDATED:
+        case EVENT_SENSOR_READ:
           if(_model.state.loopUpdate)
           {
             update();
diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h
index 99839bd3..6142b4de 100644
--- a/lib/Espfc/src/SensorManager.h
+++ b/lib/Espfc/src/SensorManager.h
@@ -50,13 +50,13 @@ class SensorManager
         case EVENT_MAG_READ:
           _mag.filter();
           return 1;
-        case EVENT_SENSOR_READ:
+        case EVENT_BBLOG_UPDATED:
           if(_model.state.imuUpdate)
           {
             _fusion.update();
             _model.state.imuUpdate = false;
+            _model.state.appQueue.send(Event(EVENT_IMU_UPDATED));
           }
-          _model.state.appQueue.send(Event(EVENT_IMU_UPDATED));
           return 1;
         default:
           break;

From b7621b42b38fd9076b473d9192572e92834540c9 Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Fri, 25 Aug 2023 12:36:25 +0200
Subject: [PATCH 3/7] optimize pid loop jitter + bblog flushing fix

---
 lib/Espfc/src/Blackbox.h          |  2 +-
 lib/Espfc/src/Sensor/GyroSensor.h | 24 +++++++++++-------------
 lib/Espfc/src/SensorManager.h     |  2 ++
 src/main.cpp                      |  6 +++++-
 4 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
index 93831fcc..80bc3f23 100644
--- a/lib/Espfc/src/Blackbox.h
+++ b/lib/Espfc/src/Blackbox.h
@@ -24,7 +24,7 @@ class BlackboxBuffer
     void write(uint8_t c)
     {
       _data[_idx++] = c;
-      if(c >= SIZE) flush();
+      if(_idx >= SIZE) flush();
     }
 
     void flush()
diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
index a3b3b313..3cf1c918 100644
--- a/lib/Espfc/src/Sensor/GyroSensor.h
+++ b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -107,6 +107,8 @@ class GyroSensor: public BaseSensor
 
       _model.state.gyroScaled = _model.state.gyro;
 
+      bool dynNotchEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0;
+
       // filtering
       for(size_t i = 0; i < 3; ++i)
       {
@@ -144,12 +146,15 @@ class GyroSensor: public BaseSensor
         {
           _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i]));
         }
-      }
 
-      filterDynNotch();
+        if(dynNotchEnabled)
+        {
+          for(size_t p = 0; p < _model.config.dynamicFilter.width; p++)
+          {
+            _model.state.gyro.set(i, _model.state.gyroDynNotchFilter[i][p].update(_model.state.gyro[i]));
+          }
+        }
 
-      for(size_t i = 0; i < 3; ++i)
-      {
         if(_model.config.debugMode == DEBUG_GYRO_FILTERED)
         {
           _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i]));
@@ -163,8 +168,7 @@ class GyroSensor: public BaseSensor
       return 1;
     }
 
-  private:
-    void filterDynNotch()
+    void dynNotchAnalyze()
     {
       bool enabled = _model.isActive(FEATURE_DYNAMIC_FILTER);
       bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0;
@@ -239,17 +243,11 @@ class GyroSensor: public BaseSensor
             }
           }
 #endif
-          if(enabled)
-          {
-            for(size_t p = 0; p < peakCount; p++)
-            {
-              _model.state.gyro.set(i, _model.state.gyroDynNotchFilter[i][p].update(_model.state.gyro[i]));
-            }
-          }
         }
       }
     }
 
+  private:
     void calibrate()
     {
       switch(_model.state.gyroCalibrationState)
diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h
index 6142b4de..a8c65e80 100644
--- a/lib/Espfc/src/SensorManager.h
+++ b/lib/Espfc/src/SensorManager.h
@@ -51,6 +51,7 @@ class SensorManager
           _mag.filter();
           return 1;
         case EVENT_BBLOG_UPDATED:
+          _gyro.dynNotchAnalyze();
           if(_model.state.imuUpdate)
           {
             _fusion.update();
@@ -129,6 +130,7 @@ class SensorManager
     // sub task
     int updateDelayed()
     {
+      _gyro.dynNotchAnalyze();
       int status = _accel.update();
       if(!status)
       {
diff --git a/src/main.cpp b/src/main.cpp
index 2302b15b..bd0e738d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -51,7 +51,11 @@ Espfc::Espfc espfc;
     }
     void loop()
     {
-      espfc.update();
+      const uint32_t timeout = millis() + 200;
+      while(millis() < timeout)
+      {
+        espfc.update();
+      }
     }
 
   #elif defined(ESPFC_MULTI_CORE_RP2040)

From b4c1487b8bba14beadd6430db3225c83228a8ed9 Mon Sep 17 00:00:00 2001
From: "nyoman.gunawan" <nyoman.gunawan@bprlestari.com>
Date: Sun, 27 Aug 2023 10:09:34 +0800
Subject: [PATCH 4/7] add gyro MPU6500

---
 README.md                          |   6 +-
 lib/Espfc/src/Device/GyroMPU6500.h | 102 +++++++++++++++++++++++++++++
 lib/Espfc/src/Hardware.h           |   4 ++
 platformio.ini                     |   4 +-
 4 files changed, 111 insertions(+), 5 deletions(-)
 create mode 100644 lib/Espfc/src/Device/GyroMPU6500.h

diff --git a/README.md b/README.md
index f234962f..68a0d1d5 100644
--- a/README.md
+++ b/README.md
@@ -82,9 +82,9 @@ After flashing you need to configure few things first:
 |----------------:|--------:|------:|-------:|
 | MPU6050/I2C     | Yes     |   Yes |    Yes |
 | MPU6000/SPI     | -       |     ? |      ? |
-| MPU6500/I2C     | ?       |     ? |      ? |
-| MPU6500/SPI     | -       |     ? |      ? |
-| MPU9250/I2C     | ?       |     ? |      ? |
+| MPU6500/I2C     | ?       |   Yes |      ? |
+| MPU6500/SPI     | -       |   Yes |    Yes |
+| MPU9250/I2C     | Yes     |   Yes |    Yes |
 | MPU9250/SPI     | -       |   Yes |    Yes |
 | BMP280/I2C      | Yes     |   Yes |    Yes |
 | BMP280/SPI      | -       |   Yes |    Yes |
diff --git a/lib/Espfc/src/Device/GyroMPU6500.h b/lib/Espfc/src/Device/GyroMPU6500.h
new file mode 100644
index 00000000..20adb40f
--- /dev/null
+++ b/lib/Espfc/src/Device/GyroMPU6500.h
@@ -0,0 +1,102 @@
+#ifndef _ESPFC_DEVICE_GYRO_MPU6500_H_
+#define _ESPFC_DEVICE_GYRO_MPU6500_H_
+
+#include "BusDevice.h"
+#include "GyroMPU6050.h"
+#include "helper_3dmath.h"
+#include "Debug_Espfc.h"
+
+#define MPU6500_USER_CTRL         0x6A
+#define MPU6500_I2C_MST_EN        0x20
+#define MPU6500_I2C_IF_DIS        0x10
+#define MPU6500_I2C_MST_400       0x0D
+#define MPU6500_I2C_MST_500       0x09
+#define MPU6500_I2C_MST_CTRL      0x24
+#define MPU6500_I2C_MST_RESET     0x02
+
+#define MPU6500_ACCEL_CONF2       0x1D
+
+#define MPU6500_WHOAMI_DEFAULT_VALUE 0x70
+
+namespace Espfc {
+
+namespace Device {
+
+class GyroMPU6500: public GyroMPU6050
+{
+  public:
+    int begin(BusDevice * bus) override
+    {
+      return begin(bus, MPU6050_DEFAULT_ADDRESS);
+    }
+
+    int begin(BusDevice * bus, uint8_t addr) override
+    {
+      setBus(bus, addr);
+
+      if(!testConnection()) return 0;
+
+      _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_RESET);
+      delay(100);
+
+      setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+
+      setSleepEnabled(false);
+      delay(100);      
+
+      // reset I2C master
+      //_bus->writeByte(_addr, MPU6500_USER_CTRL, MPU6500_I2C_MST_RESET);
+
+      // disable I2C master to reset slave registers allocation
+      _bus->writeByte(_addr, MPU6500_USER_CTRL, 0);
+      //delay(100);
+
+      // temporary force 1k sample rate for mag initiation, will be overwritten in GyroSensor
+      setDLPFMode(GYRO_DLPF_188);
+      setRate(9); // 1000 / (9+1) = 100hz
+      delay(100);
+
+      // enable I2C master mode, and disable I2C if SPI
+      if(_bus->isSPI())
+      {
+        _bus->writeByte(_addr, MPU6500_USER_CTRL, MPU6500_I2C_MST_EN | MPU6500_I2C_IF_DIS);
+      }
+      else
+      {
+        _bus->writeByte(_addr, MPU6500_USER_CTRL, MPU6500_I2C_MST_EN);
+      }
+      //delay(100);
+
+      // set the I2C bus speed to 400 kHz
+      _bus->writeByte(_addr, MPU6500_I2C_MST_CTRL, MPU6500_I2C_MST_400);
+      //delay(100);
+
+      return 1;
+    }
+
+    GyroDeviceType getType() const override
+    {
+      return GYRO_MPU6500;
+    }
+
+    void setDLPFMode(uint8_t mode) override
+    {
+      GyroMPU6050::setDLPFMode(mode);
+      _bus->writeByte(_addr, MPU6500_ACCEL_CONF2, mode);
+    }
+
+
+    bool testConnection() override
+    {
+      uint8_t whoami = 0;
+      _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
+      //D("MPU6500:whoami", _addr, whoami);
+      return whoami == MPU6500_WHOAMI_DEFAULT_VALUE;
+    }
+};
+
+}
+
+}
+
+#endif
diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h
index 6fc6a6a9..c9a97bf7 100644
--- a/lib/Espfc/src/Hardware.h
+++ b/lib/Espfc/src/Hardware.h
@@ -13,6 +13,7 @@
 #endif
 #include "Device/GyroDevice.h"
 #include "Device/GyroMPU6050.h"
+#include "Device/GyroMPU6500.h"
 #include "Device/GyroMPU9250.h"
 #include "Device/GyroLSM6DSO.h"
 #include "Device/GyroICM20602.h"
@@ -33,6 +34,7 @@ namespace {
   static Espfc::Device::BusI2C i2cBus(WireInstance);
 #endif
   static Espfc::Device::GyroMPU6050 mpu6050;
+  static Espfc::Device::GyroMPU6500 mpu6500;
   static Espfc::Device::GyroMPU9250 mpu9250;
   static Espfc::Device::GyroLSM6DSO lsm6dso;
   static Espfc::Device::GyroICM20602 icm20602;
@@ -88,6 +90,7 @@ class Hardware
         digitalWrite(_model.config.pin[PIN_SPI_CS0], HIGH);
         pinMode(_model.config.pin[PIN_SPI_CS0], OUTPUT);
         if(!detectedGyro && detectDevice(mpu9250, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu9250;
+        if(!detectedGyro && detectDevice(mpu6500, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu6500;
         if(!detectedGyro && detectDevice(icm20602, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm20602;
         if(!detectedGyro && detectDevice(lsm6dso, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &lsm6dso;
       }
@@ -96,6 +99,7 @@ class Hardware
       if(_model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1)
       {
         if(!detectedGyro && detectDevice(mpu9250, i2cBus)) detectedGyro = &mpu9250;
+        if(!detectedGyro && detectDevice(mpu6500, i2cBus)) detectedGyro = &mpu6500;
         if(!detectedGyro && detectDevice(icm20602, i2cBus)) detectedGyro = &icm20602;
         if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050;
         if(!detectedGyro && detectDevice(lsm6dso, i2cBus)) detectedGyro = &lsm6dso;
diff --git a/platformio.ini b/platformio.ini
index c734cbb7..028384b5 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -31,9 +31,9 @@ esp8266_monitor_port = /dev/ttyUSB0
 esp8266_monitor_speed = 115200
 esp8266_build_flags =
 
-esp32_upload_port = /dev/ttyUSB0
+esp32_upload_port = COM3
 esp32_upload_speed = 921600
-esp32_monitor_port = /dev/ttyUSB0
+esp32_monitor_port = COM3
 esp32_monitor_speed = 115200
 esp32_build_flags =
 

From 330964f7c49619b1cc41cd3777f86d3a81761ee9 Mon Sep 17 00:00:00 2001
From: "nyoman.gunawan" <nyoman.gunawan@bprlestari.com>
Date: Tue, 29 Aug 2023 04:50:13 +0800
Subject: [PATCH 5/7] change platformio.ini

---
 platformio.ini | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 028384b5..c734cbb7 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -31,9 +31,9 @@ esp8266_monitor_port = /dev/ttyUSB0
 esp8266_monitor_speed = 115200
 esp8266_build_flags =
 
-esp32_upload_port = COM3
+esp32_upload_port = /dev/ttyUSB0
 esp32_upload_speed = 921600
-esp32_monitor_port = COM3
+esp32_monitor_port = /dev/ttyUSB0
 esp32_monitor_speed = 115200
 esp32_build_flags =
 

From f413035c3a1ec0acd1a88d82e62e66ce6b0ae71c Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Thu, 31 Aug 2023 11:49:20 +0200
Subject: [PATCH 6/7] gyro align msp fix

---
 lib/Espfc/src/Cli.h                |  4 ++--
 lib/Espfc/src/ModelConfig.h        |  3 ++-
 lib/Espfc/src/Msp/MspProcessor.h   | 24 +++++++++++-------------
 lib/Espfc/src/Sensor/AccelSensor.h |  2 +-
 4 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h
index e8392820..393f4014 100644
--- a/lib/Espfc/src/Cli.h
+++ b/lib/Espfc/src/Cli.h
@@ -357,7 +357,7 @@ class Cli
                                                   PSTR("CRSF_LINK_STATISTICS_UPLINK"), PSTR("CRSF_LINK_STATISTICS_PWR"), PSTR("CRSF_LINK_STATISTICS_DOWN"), PSTR("BARO"), PSTR("GPS_RESCUE_THROTTLE_PID"), 
                                                   PSTR("DYN_IDLE"), PSTR("FF_LIMIT"), PSTR("FF_INTERPOLATED"), PSTR("BLACKBOX_OUTPUT"), PSTR("GYRO_SAMPLE"), PSTR("RX_TIMING"), NULL };
       static const char* filterTypeChoices[] = { PSTR("PT1"), PSTR("BIQUAD"), PSTR("PT2"), PSTR("PT3"), PSTR("NOTCH"), PSTR("NOTCH_DF1"), PSTR("BPF"), PSTR("FIR2"), PSTR("MEDIAN3"), PSTR("NONE"), NULL };
-      static const char* alignChoices[]      = { PSTR("DEFAULT"), PSTR("CW0"), PSTR("CW90"), PSTR("CW180"), PSTR("CW270"), PSTR("CW0_FLIP"), PSTR("CW90_FLIP"), PSTR("CW180_FLIP"), PSTR("CW270_FLIP"), NULL };
+      static const char* alignChoices[]      = { PSTR("DEFAULT"), PSTR("CW0"), PSTR("CW90"), PSTR("CW180"), PSTR("CW270"), PSTR("CW0_FLIP"), PSTR("CW90_FLIP"), PSTR("CW180_FLIP"), PSTR("CW270_FLIP"), PSTR("CUSTOM"), NULL };
       static const char* mixerTypeChoices[]  = { PSTR("NONE"), PSTR("TRI"), PSTR("QUADP"), PSTR("QUADX"), PSTR("BI"),
                                                  PSTR("GIMBAL"), PSTR("Y6"), PSTR("HEX6"), PSTR("FWING"), PSTR("Y4"),
                                                  PSTR("HEX6X"), PSTR("OCTOX8"), PSTR("OCTOFLATP"), PSTR("OCTOFLATX"), PSTR("AIRPLANE"),
@@ -413,7 +413,7 @@ class Cli
 
         Param(PSTR("accel_bus"), &c.accelBus, busDevChoices),
         Param(PSTR("accel_dev"), &c.accelDev, gyroDevChoices),
-        Param(PSTR("accel_align"), &c.accelAlign, alignChoices),
+        //Param(PSTR("accel_align"), &c.accelAlign, alignChoices),
         Param(PSTR("accel_lpf_type"), &c.accelFilter.type, filterTypeChoices),
         Param(PSTR("accel_lpf_freq"), &c.accelFilter.freq),
         Param(PSTR("accel_offset_x"), &c.accelBias[0]),
diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
index 644eac3c..fea2307a 100644
--- a/lib/Espfc/src/ModelConfig.h
+++ b/lib/Espfc/src/ModelConfig.h
@@ -54,7 +54,8 @@ enum SensorAlign {
   ALIGN_CW0_DEG_FLIP   = 5,
   ALIGN_CW90_DEG_FLIP  = 6,
   ALIGN_CW180_DEG_FLIP = 7,
-  ALIGN_CW270_DEG_FLIP = 8
+  ALIGN_CW270_DEG_FLIP = 8,
+  ALIGN_CUSTOM         = 9,
 };
 
 enum FusionMode {
diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h
index 43f10ba2..edf1b9c0 100644
--- a/lib/Espfc/src/Msp/MspProcessor.h
+++ b/lib/Espfc/src/Msp/MspProcessor.h
@@ -490,12 +490,13 @@ class MspProcessor
 
         case MSP_SENSOR_ALIGNMENT:
           r.writeU8(_model.config.gyroAlign); // gyro align
-          r.writeU8(_model.config.gyroAlign); // acc align
+          r.writeU8(_model.config.gyroAlign); // acc align, Starting with 4.0 gyro and acc alignment are the same
           r.writeU8(_model.config.magAlign);  // mag align
           //1.41+
-          r.writeU8(1); // gyro detection mask GYRO_1_MASK
-          r.writeU8(_model.config.gyroAlign);
-          r.writeU8(0); // default align
+          r.writeU8(_model.state.gyroPresent ? 1 : 0); // gyro detection mask GYRO_1_MASK
+          r.writeU8(0); // gyro_to_use
+          r.writeU8(_model.config.gyroAlign); // gyro 1
+          r.writeU8(0); // gyro 2
           break;
 
         case MSP_SET_SENSOR_ALIGNMENT:
@@ -503,17 +504,14 @@ class MspProcessor
             uint8_t gyroAlign = m.readU8(); // gyro align
             m.readU8(); // discard deprecated acc align
             _model.config.magAlign = m.readU8(); // mag align
+            // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
             if(m.remain() >= 3)
             {
-              m.readU8(); // gyro to use
-              _model.config.gyroAlign = m.readU8(); // gyro 1 align
+              m.readU8(); // gyro_to_use
+              gyroAlign = m.readU8(); // gyro 1 align
               m.readU8(); // gyro 2 align
             }
-            else
-            {
-              _model.config.gyroAlign = gyroAlign;
-            }
-            _model.config.accelAlign = _model.config.gyroAlign;
+            _model.config.gyroAlign = gyroAlign;
           }
           break;
 
@@ -954,7 +952,7 @@ class MspProcessor
           r.writeU16(_model.config.output.dshotIdle);
           r.writeU8(0);    // 32k gyro
           r.writeU8(0);    // PWM inversion
-          r.writeU8(0);    // gyro to use: {1:0, 2:1. 2:both}
+          r.writeU8(0);    // gyro_to_use: {1:0, 2:1. 2:both}
           r.writeU8(0);    // gyro high fsr (flase)
           r.writeU8(48);   // gyro cal threshold
           r.writeU16(125); // gyro cal duration (1.25s)
@@ -980,7 +978,7 @@ class MspProcessor
             m.readU8();  // PWM inversion
           }
           if(m.remain() >= 8) {
-            m.readU8();  // gyro to use
+            m.readU8();  // gyro_to_use
             m.readU8();  // gyro high fsr
             m.readU8();  // gyro cal threshold
             m.readU16(); // gyro cal duration
diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h
index b7fe6a0f..40518fc4 100644
--- a/lib/Espfc/src/Sensor/AccelSensor.h
+++ b/lib/Espfc/src/Sensor/AccelSensor.h
@@ -69,7 +69,7 @@ class AccelSensor: public BaseSensor
 
       Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER);
 
-      align(_model.state.accelRaw, _model.config.accelAlign);
+      align(_model.state.accelRaw, _model.config.gyroAlign);
 
       _model.state.accel = (VectorFloat)_model.state.accelRaw * _model.state.accelScale;
       for(size_t i = 0; i < 3; i++)

From f21c857fb840984fec42a221e450558d407a0101 Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Thu, 31 Aug 2023 12:08:16 +0200
Subject: [PATCH 7/7] cherry pick 754385e - fft stats

---
 lib/Espfc/src/Sensor/GyroSensor.h | 2 ++
 lib/Espfc/src/Stats.h             | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
index 3cf1c918..b57e2ff1 100644
--- a/lib/Espfc/src/Sensor/GyroSensor.h
+++ b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -170,6 +170,8 @@ class GyroSensor: public BaseSensor
 
     void dynNotchAnalyze()
     {
+      Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT);
+
       bool enabled = _model.isActive(FEATURE_DYNAMIC_FILTER);
       bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0;
       bool debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME;
diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h
index d45ebfbd..8591a94a 100644
--- a/lib/Espfc/src/Stats.h
+++ b/lib/Espfc/src/Stats.h
@@ -9,6 +9,7 @@ namespace Espfc {
 enum StatCounter : int8_t {
   COUNTER_GYRO_READ,
   COUNTER_GYRO_FILTER,
+  COUNTER_GYRO_FFT,
   COUNTER_ACCEL_READ,
   COUNTER_ACCEL_FILTER,
   COUNTER_MAG_READ,
@@ -156,6 +157,7 @@ class Stats
       {
         case COUNTER_GYRO_READ:    return PSTR(" gyro_r");
         case COUNTER_GYRO_FILTER:  return PSTR(" gyro_f");
+        case COUNTER_GYRO_FFT:     return PSTR(" gyro_a");
         case COUNTER_ACCEL_READ:   return PSTR("  acc_r");
         case COUNTER_ACCEL_FILTER: return PSTR("  acc_f");
         case COUNTER_MAG_READ:     return PSTR("  mag_r");