Merge pull request #74 from rtlopez/develop

release 0.2.0-alpha5
rtlopez · Aug 31, 2023 · fffab06 · fffab06
2 parents ede2f6d + f21c857
commit fffab06
Show file tree

Hide file tree

Showing 21 changed files with 288 additions and 125 deletions.
diff --git 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/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/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/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()
@@ -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/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/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/Device/GyroMPU6500.h 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/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
@@ -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
@@ -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/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/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 {