Merge pull request #51 from rtlopez/i2c-fixes

ESP32 I2C improvements

lib/EspWire/src/EspWire.h

@@ -29,7 +29,7 @@
 #include <Stream.h>
 #endif
 
-#define ESPWIRE_BUFFER_LENGTH 32
+#define ESPWIRE_BUFFER_LENGTH 64
 
 class EspTwoWire : public Stream
 {

lib/EspWire/src/esp_twi.cpp

@@ -37,7 +37,8 @@ static uint32_t esp_twi_clockStretchLimit;
   #define SCL_HIGH()  (GPEC = (1 << esp_twi_scl))
   #define SCL_READ()  ((GPI & (1 << esp_twi_scl)) != 0)
 #elif defined(ESP32)
-  #define SDA_LOW()   (GPIO.enable_w1ts = (1 << esp_twi_sda)) //Enable SDA (becomes output and since GPO is 0 for the pin, it will pull the line low)
+  //#define SDA_LOW()   (GPIO.enable_w1ts = (1 << esp_twi_sda)) //Enable SDA (becomes output and since GPO is 0 for the pin, it will pull the line low)
+  static inline void SDA_LOW() { GPIO.enable_w1ts = 1 << esp_twi_sda; GPIO.out_w1tc = 1 << esp_twi_sda; }
   #define SDA_HIGH()  (GPIO.enable_w1tc = (1 << esp_twi_sda)) //Disable SDA (becomes input and since it has pullup it will go high)
   #define SDA_READ()  ((GPIO.in & (1 << esp_twi_sda)) != 0)
   #define SCL_LOW()   (GPIO.enable_w1ts = (1 << esp_twi_scl))
@@ -97,10 +98,9 @@ void esp_twi_setClock(unsigned int freq){
   else if(freq < 400000) esp_twi_dcount = 1;//about 400KHz
   else esp_twi_dcount = 0;//about 400KHz
 #elif F_CPU == FCPU240
-  if(freq <= 60000) esp_twi_dcount = 280;      //about  50kHz
-  else if(freq <  80000) esp_twi_dcount = 180; //about  80kHz
-  else if(freq < 100000) esp_twi_dcount = 140; //about 100kHz
-  else if(freq < 150000) esp_twi_dcount = 90;  //about 150kHz
+  if(freq < 50000) esp_twi_dcount = 300;      //about  50kHz
+  else if(freq < 100000) esp_twi_dcount = 190; //about 100kHz
+  else if(freq < 150000) esp_twi_dcount = 133; //about 150kHz
   else if(freq < 200000) esp_twi_dcount = 65;  //about 200kHz
   else if(freq < 250000) esp_twi_dcount = 50;  //about 250kHz
   else if(freq < 300000) esp_twi_dcount = 39;  //about 300kHz
@@ -110,8 +110,8 @@ void esp_twi_setClock(unsigned int freq){
   else if(freq < 700000) esp_twi_dcount = 11;  //about 700kHz
   else if(freq < 800000) esp_twi_dcount = 8;   //about 800kHz
   else if(freq < 900000) esp_twi_dcount = 7;   //about 900kHz
-  else if(freq < 1000000) esp_twi_dcount = 4;  //about 1.0MHz
-  else if(freq < 1100000) esp_twi_dcount = 3;  //about 1.1MHz
+  else if(freq < 1000000) esp_twi_dcount = 5;  //about 1.0MHz
+  else if(freq < 1100000) esp_twi_dcount = 4;  //about 1.1MHz
   else if(freq < 1200000) esp_twi_dcount = 2;  //about 1.2MHz
   else esp_twi_dcount = 1;                     //above 1.2MHz
 #elif F_CPU == FCPU133 // 133 mhz

lib/Espfc/src/Device/BusDevice.h

@@ -21,36 +21,48 @@ namespace Device {
 class BusDevice
 {
   public:
+    BusDevice(): _timeout(ESPFC_BUS_TIMEOUT) {}
+
     virtual BusType getType() const = 0;
 
-    virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) = 0;
+    virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) = 0;
+
+    virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) = 0;
 
-    virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) = 0;
+    virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) = 0;
+
+    bool isSPI() const
+    {
+      return getType() == BUS_SPI;
+    }
 
-    virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) = 0;
+    void setTimeout(uint32_t t)
+    {
+      _timeout = t;
+    }
 
-    int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT)
+    int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data)
     {
-      return read(devAddr, regAddr, 1, data, timeout);
+      return read(devAddr, regAddr, 1, data);
     }
 
     bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
     {
       return write(devAddr, regAddr, 1, &data);
     }
 
-    int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT)
+    int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data)
     {
       uint8_t b;
-      uint8_t count = readByte(devAddr, regAddr, &b, timeout);
+      uint8_t count = readByte(devAddr, regAddr, &b);
       *data = b & (1 << bitNum);
       return count;
     }
 
-    int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT)
+    int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
     {
       uint8_t count, b;
-      if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0)
+      if ((count = readByte(devAddr, regAddr, &b)) != 0)
       {
         uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
         b &= mask;
@@ -111,6 +123,9 @@ class BusDevice
     }
 
     std::function<void(void)> onError;
+
+  protected:
+    uint32_t _timeout;
 };
 
 }

lib/Espfc/src/Device/BusI2C.h

@@ -19,15 +19,16 @@ class BusI2C: public BusDevice
       if(sda == -1 || scl == -1) return 0;
 
       targetI2CInit(_dev, sda, scl, speed);
+
       return 1;
     }
 
-    int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override
+    int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override
     {
-      return read(devAddr, regAddr, length, data, timeout);
+      return read(devAddr, regAddr, length, data);
     }
 
-    int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override
+    int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override
     {
       int8_t count = 0;
       uint32_t t1 = millis();
@@ -39,21 +40,21 @@ class BusI2C: public BusDevice
       _dev.endTransmission();
       _dev.requestFrom(devAddr, length);
 
-      for (; _dev.available() && (timeout == 0 || millis() - t1 < timeout); count++)
+      for (; _dev.available() && (_timeout == 0 || millis() - t1 < _timeout); count++)
       {
         data[count] = _dev.read();
         //D("i2c:r1", count, data[count]);
       }
 
       //D("i2c:r3", length, count);
-      if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
+      if (_timeout > 0 && millis() - t1 >= _timeout && count < length) count = -1; // timeout
 
       if(onError && count != length) onError();
 
       return count;
     }
 
-    bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) override
+    bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override
     {
       //Serial.print("I2C W "); Serial.print(devAddr, HEX); Serial.print(' '); Serial.print(regAddr, HEX); Serial.print(' '); Serial.println(length);
 

lib/Espfc/src/Device/BusSPI.h

@@ -29,29 +29,29 @@ class BusSPI: public BusDevice
       return 1;
     }
 
-    int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override
+    int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override
     {
       //D("spi:r", regAddr, length);
       transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_NORMAL);
       return length;
     }
 
-    int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override
+    int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override
     {
       //D("spi:r", regAddr, length);
       transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_FAST);
       return length;
     }
 
-    bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) override
+    bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override
     {
       //D("spi:w", regAddr, length, *data);
       transfer(devAddr, regAddr & SPI_WRITE, length, data, NULL, SPI_SPEED_NORMAL);
       return true;
     }
 
   private:
-    void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *in, uint8_t *out, uint32_t speed)
+    void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed)
     {
       _dev.beginTransaction(SPISettings(speed, MSBFIRST, SPI_MODE0));
       digitalWrite(devAddr, LOW);

lib/Espfc/src/Device/GyroMPU9250.h

@@ -55,7 +55,7 @@ class GyroMPU9250: public GyroMPU6050
       delay(100);
 
       // enable I2C master mode, and disable I2C if SPI
-      if(_bus->getType() == BUS_SPI)
+      if(_bus->isSPI())
       {
         _bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_EN | MPU9250_I2C_IF_DIS);
       }

lib/Espfc/src/Math/Utils.h

@@ -51,6 +51,17 @@ class Peak
     return value;
   }
 
+  int alignToClock(uint32_t clock, uint32_t maxFreq)
+  {
+    uint32_t result = clock;
+    uint32_t div = 1;
+    while(result > maxFreq)
+    {
+      result = clock / ++div;
+    }
+    return result;
+  }
+
   constexpr float pi()
   {
     return 3.14159265358979f;

lib/Espfc/src/Model.h

@@ -265,16 +265,18 @@ class Model
 
     void sanitize()
     {
-      state.gyroRate = 1000;
-      int loopSyncMax = 1;
-      if(config.magDev != MAG_NONE || config.baroDev != BARO_NONE) loopSyncMax /= 2;
-
       // for spi gyro allow full speed mode
-      if (state.gyroDev && state.gyroDev->getBus()->getType() == BUS_SPI)
+      if (state.gyroDev && state.gyroDev->getBus()->isSPI())
       {
-        loopSyncMax = ESPFC_GYRO_DENOM_MAX; // max 8kHz
-        state.gyroRate = state.gyroClock;
+        state.gyroRate = Math::alignToClock(state.gyroClock, ESPFC_GYRO_SPI_RATE_MAX);
       }
+      else
+      {
+        state.gyroRate = Math::alignToClock(state.gyroClock, ESPFC_GYRO_I2C_RATE_MAX);
+      }
+
+      int loopSyncMax = 1;
+      //if(config.magDev != MAG_NONE || config.baroDev != BARO_NONE) loopSyncMax /= 2;
 
       config.loopSync = std::max((int)config.loopSync, loopSyncMax);
       state.loopRate = state.gyroRate / config.loopSync;

lib/Espfc/src/Target/TargetESP32.h

@@ -63,7 +63,7 @@
 #define ESPFC_I2C_0
 #define ESPFC_I2C_0_SCL 22
 #define ESPFC_I2C_0_SDA 21
-//#define ESPFC_I2C_0_SOFT
+#define ESPFC_I2C_0_SOFT
 
 #define ESPFC_BUZZER
 #define ESPFC_BUZZER_PIN 0
@@ -77,7 +77,10 @@
 #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER)
 
 #define ESPFC_GUARD 0
-#define ESPFC_GYRO_DENOM_MAX 1
+
+#define ESPFC_GYRO_I2C_RATE_MAX 2000
+#define ESPFC_GYRO_SPI_RATE_MAX 8000
+
 //#define ESPFC_LOGGER_FS // doesn't compile on ESP32
 
 #define ESPFC_FREE_RTOS
@@ -146,6 +149,7 @@ template<typename T>
 inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed)
 {
   dev.begin(sda, scl, speed);
+  dev.setTimeout(50);
   return 1;
 }
 

lib/Espfc/src/Target/TargetESP8266.h

@@ -50,7 +50,10 @@
 #define ESPFC_FEATURE_MASK (FEATURE_RX_PPM | FEATURE_DYNAMIC_FILTER)
 
 #define ESPFC_GUARD 1
-#define ESPFC_GYRO_DENOM_MAX 4
+
+#define ESPFC_GYRO_I2C_RATE_MAX 1000
+#define ESPFC_GYRO_SPI_RATE_MAX 1000
+
 #define ESPFC_WIFI_ALT
 
 //#define ESPFC_LOGGER_FS // deprecated

lib/Espfc/src/Target/TargetRP2040.h

@@ -66,7 +66,9 @@
 #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER)
 
 #define ESPFC_GUARD 0
-#define ESPFC_GYRO_DENOM_MAX 1
+
+#define ESPFC_GYRO_I2C_RATE_MAX 2000
+#define ESPFC_GYRO_SPI_RATE_MAX 2000
 
 #define ESPFC_MULTI_CORE
 #define ESPFC_MULTI_CORE_RP2040

lib/Espfc/src/Target/TargetUnitTest.h

@@ -13,4 +13,6 @@
 #define ESPFC_FEATURE_MASK (0)
 
 #define ESPFC_GUARD 1
-#define ESPFC_GYRO_DENOM_MAX 1
+
+#define ESPFC_GYRO_I2C_RATE_MAX 2000
+#define ESPFC_GYRO_SPI_RATE_MAX 8000

test/test_fc/test_fc.cpp

@@ -110,11 +110,11 @@ void test_model_gyro_init_1k_256dlpf()
   model.begin();
 
   TEST_ASSERT_EQUAL_INT32(8000, model.state.gyroClock);
-  TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroRate);
-  TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroTimer.rate);
-  TEST_ASSERT_EQUAL_INT32(1000, model.state.loopRate);
-  TEST_ASSERT_EQUAL_INT32(1000, model.state.loopTimer.rate);
-  TEST_ASSERT_EQUAL_INT32(1000, model.state.mixerTimer.rate);
+  TEST_ASSERT_EQUAL_INT32(2000, model.state.gyroRate);
+  TEST_ASSERT_EQUAL_INT32(2000, model.state.gyroTimer.rate);
+  TEST_ASSERT_EQUAL_INT32(2000, model.state.loopRate);
+  TEST_ASSERT_EQUAL_INT32(2000, model.state.loopTimer.rate);
+  TEST_ASSERT_EQUAL_INT32(2000, model.state.mixerTimer.rate);
 }
 
 void test_model_gyro_init_1k_188dlpf()
@@ -177,13 +177,13 @@ void test_model_outer_pid_init()
   model.config.pid[PID_LEVEL]  = { .P = 100u, .I = 100u, .D = 100u, .F = 100 };
   model.begin();
 
-  TEST_ASSERT_FLOAT_WITHIN(   0.1f, 1000.0f, model.state.outerPid[PID_ROLL].rate);
+  TEST_ASSERT_FLOAT_WITHIN(   0.1f, 2000.0f, model.state.outerPid[PID_ROLL].rate);
   TEST_ASSERT_FLOAT_WITHIN(0.0001f,   10.0f, model.state.outerPid[PID_ROLL].Kp);
   TEST_ASSERT_FLOAT_WITHIN(0.0001f,   10.0f, model.state.outerPid[PID_ROLL].Ki);
   TEST_ASSERT_FLOAT_WITHIN(0.0001f,    0.1f, model.state.outerPid[PID_ROLL].Kd);
   TEST_ASSERT_FLOAT_WITHIN(0.0001f,    0.1f, model.state.outerPid[PID_ROLL].Kf);
 
-  TEST_ASSERT_FLOAT_WITHIN(   0.1f, 1000.0f, model.state.outerPid[PID_PITCH].rate);
+  TEST_ASSERT_FLOAT_WITHIN(   0.1f, 2000.0f, model.state.outerPid[PID_PITCH].rate);
   TEST_ASSERT_FLOAT_WITHIN(0.0001f,   10.0f, model.state.outerPid[PID_PITCH].Kp);
   TEST_ASSERT_FLOAT_WITHIN(0.0001f,   10.0f, model.state.outerPid[PID_PITCH].Ki);
   TEST_ASSERT_FLOAT_WITHIN(0.0001f,    0.1f, model.state.outerPid[PID_PITCH].Kd);