gy87 handling

rtlopez · Nov 17, 2023 · f3259ec · f3259ec
1 parent abb57ae
commit f3259ec
Show file tree

Hide file tree

Showing 17 changed files with 155 additions and 182 deletions.
diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
@@ -368,7 +368,7 @@ class Blackbox
           acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G;
         }
         if(_model.magActive()) {
-          mag.magADC[i] = _model.state.mag[i];
+          mag.magADC[i] = _model.state.mag[i] * 1090;
         }
         if(_model.baroActive()) {
           baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm

diff --git a/lib/Espfc/src/Device/BusDevice.h b/lib/Espfc/src/Device/BusDevice.h
@@ -51,6 +51,53 @@ class BusDevice
       return write(devAddr, regAddr, 1, &data);
     }
 
+    static bool getBit(uint8_t data, uint8_t bitNum)
+    {
+      return data & (1 << bitNum);
+    }
+
+    static uint8_t setBit(uint8_t data, uint8_t bitNum, uint8_t bitVal)
+    {
+      data = (bitVal != 0) ? (data | (1 << bitNum)) : (data & ~(1 << bitNum));
+      return data;
+    }
+
+    static uint8_t setBitsLsb(uint8_t data, uint8_t bitStart, uint8_t bitLen, uint8_t bitVal)
+    {
+      uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1);
+      bitVal <<= (bitStart - bitLen + 1); // shift data into correct position
+      bitVal &= mask; // zero all non-important bits in data
+      data &= ~(mask); // zero all important bits in existing byte
+      data |= bitVal; // combine data with existing byte
+      return data;
+    }
+
+    static uint8_t setBitsMsb(uint8_t data, uint8_t bitStart, uint8_t bitLen, uint8_t bitVal)
+    {
+      uint8_t mask = ((1 << bitLen) - 1) << bitStart;
+      bitVal <<= bitStart; // shift data into correct position
+      bitVal &= mask; // zero all non-important bits in data
+      data &= ~(mask); // zero all important bits in existing byte
+      data |= bitVal; // combine data with existing byte
+      return data;
+    }
+
+    static uint8_t getBitsLsb(uint8_t data, uint8_t bitStart, uint8_t bitLen)
+    {
+      uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1);
+      data &= mask;
+      data >>= (bitStart - bitLen + 1);
+      return data;
+    }
+
+    static uint8_t getBitsMsb(uint8_t data, uint8_t bitStart, uint8_t bitLen)
+    {
+      uint8_t mask = ((1 << bitLen) - 1);
+      data >>= bitStart;
+      data &= mask;
+      return data;
+    }
+
     int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data)
     {
       uint8_t b;

diff --git a/lib/Espfc/src/Device/GyroBMI160.h b/lib/Espfc/src/Device/GyroBMI160.h
@@ -244,14 +244,6 @@ class GyroBMI160: public GyroDevice
       //D("bmi160:whoami", _addr, whoami);
       return whoami == BMI160_CHIP_ID_DEFAULT_VALUE;
     }
-
-    void setSleepEnabled(bool enabled)
-    {
-    }
-
-    void setClockSource(uint8_t source)
-    {
-    }
 };
 
 }

diff --git a/lib/Espfc/src/Device/GyroICM20602.h b/lib/Espfc/src/Device/GyroICM20602.h
@@ -70,9 +70,6 @@
 #define ICM20602_ACONFIG_AFS_SEL_BIT         4
 #define ICM20602_ACONFIG_AFS_SEL_LENGTH      2
 
-#define ICM20602_WHO_AM_I_BIT        6
-#define ICM20602_WHO_AM_I_LENGTH     6
-
 #define ICM20602_USERCTRL_FIFO_EN_BIT            6
 #define ICM20602_USERCTRL_FIFO_RESET_BIT         2
 
@@ -95,14 +92,14 @@ class GyroICM20602: public GyroDevice
 
       if(!testConnection()) return 0;
 
+      // reset gyro
       _bus->writeByte(_addr, ICM20602_RA_PWR_MGMT_1, ICM20602_RESET);
       delay(100);
 
-      setClockSource(ICM20602_CLOCK_PLL_XGYRO);
+      // disable sleep mode and set clock source
+      _bus->writeByte(_addr, ICM20602_RA_PWR_MGMT_1, ICM20602_CLOCK_PLL_XGYRO);
       delay(15);
 
-      setSleepEnabled(false);
-
       return 1;
     }
 
@@ -160,7 +157,7 @@ class GyroICM20602: public GyroDevice
       // Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
       // where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7),
       // and 1kHz when the DLPF is enabled (see Register 26).
-      const uint8_t divider = getRate() / (rate + 1);
+      const uint8_t divider = (getRate() / rate) - 1;
       _bus->writeByte(_addr, ICM20602_RA_SMPLRT_DIV, divider);
     }
 
@@ -178,20 +175,10 @@ class GyroICM20602: public GyroDevice
     {
       uint8_t whoami = 0;
       _bus->readByte(_addr, ICM20602_RA_WHO_AM_I, &whoami);
-      //D("mpu6050:whoami", _addr, whoami);
+      //D("icm20602:whoami", _addr, whoami);
       return whoami == 0x12;
     }
 
-    void setSleepEnabled(bool enabled)
-    {
-      _bus->writeBit(_addr, ICM20602_RA_PWR_MGMT_1, ICM20602_PWR1_SLEEP_BIT, enabled);
-    }
-
-    void setClockSource(uint8_t source)
-    {
-      _bus->writeBits(_addr, ICM20602_RA_PWR_MGMT_1, ICM20602_PWR1_CLKSEL_BIT, ICM20602_PWR1_CLKSEL_LENGTH, source);
-    }
-
     uint8_t _dlpf;
 };
 

diff --git a/lib/Espfc/src/Device/GyroLSM6DSO.h b/lib/Espfc/src/Device/GyroLSM6DSO.h
@@ -165,14 +165,6 @@ class GyroLSM6DSO: public GyroDevice
       //D("lsm6dso:whoami", _addr, whoami);
       return whoami == 0x6C || whoami == 0x69;
     }
-
-    void setSleepEnabled(bool enabled)
-    {
-    }
-
-    void setClockSource(uint8_t source)
-    {
-    }
 };
 
 }

diff --git a/lib/Espfc/src/Device/GyroMPU6050.h b/lib/Espfc/src/Device/GyroMPU6050.h
@@ -1,6 +1,9 @@
 #ifndef _ESPFC_DEVICE_GYRO_MPU6050_H_
 #define _ESPFC_DEVICE_GYRO_MPU6050_H_
 
+// https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050.cpp#L1501
+// https://github.com/guywithaview/Arduino-Test/blob/master/GY87/GY87.ino
+
 #include "BusDevice.h"
 #include "GyroDevice.h"
 #include "helper_3dmath.h"
@@ -66,6 +69,7 @@
 #define MPU6050_ACCEL_FS_16         0x03
 
 #define MPU6050_RESET               0x80
+#define MPU6050_SIG_COND_RESET      0x01
 
 #define MPU6050_ACONFIG_AFS_SEL_BIT         4
 #define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
@@ -76,6 +80,16 @@
 #define MPU6050_USERCTRL_FIFO_EN_BIT            6
 #define MPU6050_USERCTRL_FIFO_RESET_BIT         2
 
+#define MPU6050_USER_CTRL         0x6A
+#define MPU6050_I2C_MST_EN        0x20
+#define MPU6050_I2C_IF_DIS        0x10
+#define MPU6050_I2C_MST_400       0x0D
+#define MPU6050_I2C_MST_500       0x09
+#define MPU6050_I2C_MST_CTRL      0x24
+#define MPU6050_I2C_MST_RESET     0x02
+
+#define MPU6050_INT_PIN_CFG       0x37
+#define MPU6050_I2C_BYPASS_EN     0x02
 
 namespace Espfc {
 
@@ -95,13 +109,49 @@ class GyroMPU6050: public GyroDevice
 
       if(!testConnection()) return 0;
 
-      _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_RESET);
+      uint8_t res = 0;
+
+      res = _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_RESET);
+      //D("mpu6050:reset", res);
       delay(100);
 
-      setClockSource(MPU6050_CLOCK_PLL_XGYRO);
-      delay(15);
+      // disable sleep mode and set clock source
+      res = _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_XGYRO);
+      //D("mpu6050:sleep_pll", res);
+      delay(10);
+
+      // reset I2C master and sensors signal path
+      res = _bus->writeByte(_addr, MPU6050_USER_CTRL, MPU6050_I2C_MST_RESET | MPU6050_SIG_COND_RESET);
+      //D("mpu6050:sig_reset", res);
+      delay(10);
+
+      // temporary force 1k gyro rate for mag initiation, will be overwritten in GyroSensor
+      setDLPFMode(GYRO_DLPF_188);
+      setRate(100);
+      delay(10);
+
+      if(_bus->isSPI())
+      {
+        // reset I2C master
+        //_bus->writeByte(_addr, MPU6050_USER_CTRL, MPU6050_I2C_MST_RESET);
+
+        // enable I2C master mode, and disable I2C
+        res = _bus->writeByte(_addr, MPU6050_USER_CTRL, MPU6050_I2C_MST_EN | MPU6050_I2C_IF_DIS);
+        //D("mpu6050:i2c_master_en", b, res);
+
+        // set the I2C bus speed to 400 kHz
+        res = _bus->writeByte(_addr, MPU6050_I2C_MST_CTRL, MPU6050_I2C_MST_400);
+        //D("mpu6050:i2c_master_speed", b, res);
+      }
+      else
+      {
+        // enable I2C bypass mode
+        res = _bus->writeByte(_addr, MPU6050_INT_PIN_CFG, MPU6050_I2C_BYPASS_EN);
+        //D("mpu6050:i2c_bypass", b, res);
+      }
+      delay(10);
 
-      setSleepEnabled(false);
+      (void)res;
 
       return 1;
     }
@@ -140,7 +190,9 @@ class GyroMPU6050: public GyroDevice
     void setDLPFMode(uint8_t mode) override
     {
       _dlpf = mode;
-      _bus->writeBits(_addr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
+      bool res = _bus->writeBits(_addr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
+      //D("mpu6050:dlpf", mode, res);
+      (void)res;
     }
 
     int getRate() const override
@@ -160,8 +212,11 @@ class GyroMPU6050: public GyroDevice
       // Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
       // where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7),
       // and 1kHz when the DLPF is enabled (see Register 26).
-      const uint8_t divider = getRate() / (rate + 1);
-      _bus->writeByte(_addr, MPU6050_RA_SMPLRT_DIV, divider);
+      // therefore: SMPLRT_DIV = (Gyroscope Output Rate / Sample Rate) - 1
+      const uint8_t divider = (getRate() / rate) - 1;
+      uint8_t res = _bus->writeByte(_addr, MPU6050_RA_SMPLRT_DIV, divider);
+      //D("mpu6050:rate", rate, divider, res);
+      (void)res;
     }
 
     void setFullScaleGyroRange(uint8_t range) override
@@ -177,19 +232,9 @@ class GyroMPU6050: public GyroDevice
     bool testConnection() override
     {
       uint8_t whoami = 0;
-      _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
-      //D("mpu6050:whoami", _addr, whoami);
-      return whoami == 0x68 || whoami == 0x72;
-    }
-
-    void setSleepEnabled(bool enabled)
-    {
-      _bus->writeBit(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
-    }
-
-    void setClockSource(uint8_t source)
-    {
-      _bus->writeBits(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
+      uint8_t len = _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
+      //D("mpu6050:whoami", _addr, whoami, len);
+      return len == 1 && (whoami == 0x68 || whoami == 0x72);
     }
 
     uint8_t _dlpf;

diff --git a/lib/Espfc/src/Device/GyroMPU6500.h b/lib/Espfc/src/Device/GyroMPU6500.h
@@ -17,7 +17,7 @@
 #define MPU6500_ACCEL_CONF2       0x1D
 
 #define MPU6500_WHOAMI_DEFAULT_VALUE 0x70
-#define MPU6555_WHOAMI_DEFAULT_VALUE 0x75
+#define MPU6500_WHOAMI_ALT_VALUE     0x75
 
 namespace Espfc {
 
@@ -26,55 +26,6 @@ namespace Device {
 class GyroMPU6500: public GyroMPU6050
 {
   public:
-    int begin(BusDevice * bus) override
-    {
-      return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND) ? 1 : 0;
-    }
-
-    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;
@@ -90,9 +41,9 @@ class GyroMPU6500: public GyroMPU6050
     bool testConnection() override
     {
       uint8_t whoami = 0;
-      _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
+      uint8_t len = _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
       //D("MPU6500:whoami", _addr, whoami);
-      return whoami == MPU6500_WHOAMI_DEFAULT_VALUE || whoami == MPU6555_WHOAMI_DEFAULT_VALUE;
+      return len == 1 && (whoami == MPU6500_WHOAMI_DEFAULT_VALUE || whoami == MPU6500_WHOAMI_ALT_VALUE);
     }
 };