add gyro bmi160 and fix gyro i2c address

README.md

@@ -93,6 +93,8 @@ After flashing you need to configure few things first:
 | AK8963/I2C      | -       |   Yes |    Yes |
 | ICM20602/I2C    | ?       |     ? |      ? |
 | ICM20602/SPI    | -       |   Yes |    Yes |
+| BMI160/I2C      | ?       |   Yes |      ? |
+| BMI160/SPI      | -       |   Yes |    Yes |
 
 ? - not tested, but should work
 

lib/Espfc/src/Device/BaroBMP280.h

@@ -5,7 +5,8 @@
 #include "BaroDevice.h"
 #include "Debug_Espfc.h"
 
-#define BMP280_DEFAULT_ADDRESS        0x77
+#define BMP280_ADDRESS_FIRST          0x76
+#define BMP280_ADDRESS_SECOND         0x77
 #define BMP280_WHOAMI_ID              0x58
 
 #define BMP280_WHOAMI_REG             0xD0
@@ -60,7 +61,7 @@ class BaroBMP280: public BaroDevice
 
     int begin(BusDevice * bus) override
     {
-      return begin(bus, BMP280_DEFAULT_ADDRESS);
+      return begin(bus, BMP280_ADDRESS_FIRST) ? 1 : begin(bus, BMP280_ADDRESS_SECOND) ? 1 : 0;
     }
 
     int begin(BusDevice * bus, uint8_t addr) override

lib/Espfc/src/Device/BusDevice.h

@@ -72,6 +72,19 @@ class BusDevice
       return count;
     }
 
+    int8_t readBitsBMI160(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)) != 0)
+      {
+        uint8_t mask = (1 << length) - 1;
+        b >>= bitStart;
+        b &= mask;
+        *data = b;
+      }
+      return count;
+    }
+
     bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
     {
       uint8_t b;
@@ -96,6 +109,22 @@ class BusDevice
       }
     }
 
+    bool writeBitsBMI160(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
+    {
+      uint8_t b = 0;
+      if (readByte(devAddr, regAddr, &b) != 0)
+      {
+        uint8_t mask = ((1 << length) - 1) << bitStart;
+        data <<= bitStart; // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+      } else {
+        return false;
+      }
+    }
+
     bool writeMask(uint8_t devAddr, uint8_t regAddr, uint8_t mask, uint8_t data)
     {
       uint8_t b = 0;

lib/Espfc/src/Device/GyroBMI160.h

@@ -0,0 +1,292 @@
+#ifndef _ESPFC_DEVICE_GYRO_BMI160_H_
+#define _ESPFC_DEVICE_GYRO_BMI160_H_
+
+#include "BusDevice.h"
+#include "GyroDevice.h"
+#include "helper_3dmath.h"
+#include "Debug_Espfc.h"
+
+#define BMI160_ADDRESS_FIRST        0x69
+#define BMI160_ADDRESS_SECOND       0x68
+#define BMI160_RA_CHIP_ID           0x00
+#define BMI160_CHIP_ID_DEFAULT_VALUE 0xD1
+
+#define BMI160_RA_GYRO_X_L          0x0C
+#define BMI160_RA_GYRO_X_H          0x0D
+#define BMI160_RA_GYRO_Y_L          0x0E
+#define BMI160_RA_GYRO_Y_H          0x0F
+#define BMI160_RA_GYRO_Z_L          0x10
+#define BMI160_RA_GYRO_Z_H          0x11
+#define BMI160_RA_ACCEL_X_L         0x12
+#define BMI160_RA_ACCEL_X_H         0x13
+#define BMI160_RA_ACCEL_Y_L         0x14
+#define BMI160_RA_ACCEL_Y_H         0x15
+#define BMI160_RA_ACCEL_Z_L         0x16
+#define BMI160_RA_ACCEL_Z_H         0x17
+
+#define BMI160_ACCEL_RATE_SEL_BIT    0
+#define BMI160_ACCEL_RATE_SEL_LEN    4
+
+#define BMI160_RA_ACCEL_CONF        0X40
+#define BMI160_RA_ACCEL_RANGE       0X41
+
+#define BMI160_GYRO_RATE_SEL_BIT    0
+#define BMI160_GYRO_RATE_SEL_LEN    4
+
+#define BMI160_RA_GYRO_CONF         0X42
+#define BMI160_RA_GYRO_RANGE        0X43
+
+#define BMI160_ACC_OFFSET_EN        6
+#define BMI160_ACC_OFFSET_LEN       1
+#define BMI160_GYR_OFFSET_EN        7
+#define BMI160_GYR_OFFSET_LEN       1
+
+#define BMI160_RA_OFFSET_0          0x71
+#define BMI160_RA_OFFSET_1          0x72
+#define BMI160_RA_OFFSET_2          0x73
+#define BMI160_RA_OFFSET_3          0x74
+#define BMI160_RA_OFFSET_4          0x75
+#define BMI160_RA_OFFSET_5          0x76
+#define BMI160_RA_OFFSET_6          0x77
+
+#define BMI160_REG_INT_EN1          0x51
+#define BMI160_INT_EN1_DRDY         0x10
+#define BMI160_REG_INT_OUT_CTRL     0x53
+#define BMI160_INT_OUT_CTRL_INT1_CONFIG 0x0A
+#define BMI160_REG_INT_MAP1         0x56
+#define BMI160_REG_INT_MAP1_INT1_DRDY 0x80
+
+#define BMI160_CMD_START_FOC        0x03
+#define BMI160_CMD_ACC_MODE_NORMAL  0x11
+#define BMI160_CMD_GYR_MODE_NORMAL  0x15
+#define BMI160_CMD_FIFO_FLUSH       0xB0
+#define BMI160_CMD_INT_RESET        0xB1
+#define BMI160_CMD_STEP_CNT_CLR     0xB2
+#define BMI160_CMD_SOFT_RESET       0xB6
+#define BMI160_CMD_SPI_MODE         0x7F
+#define BMI160_RESULT_OK            0x1
+
+#define BMI160_RA_CMD               0x7E
+
+namespace Espfc {
+
+namespace Device {
+
+class GyroBMI160: public GyroDevice
+{
+  public:
+    enum {
+        BMI160_ACCEL_RANGE_2G  = 0X03, /**<  +/-  2g range */
+        BMI160_ACCEL_RANGE_4G  = 0X05, /**<  +/-  4g range */
+        BMI160_ACCEL_RANGE_8G  = 0X08, /**<  +/-  8g range */
+        BMI160_ACCEL_RANGE_16G = 0X0C, /**<  +/- 16g range */
+    };
+
+    enum {
+        BMI160_GYRO_RANGE_2000 = 0, /**<  +/- 2000 degrees/second */
+        BMI160_GYRO_RANGE_1000,     /**<  +/- 1000 degrees/second */
+        BMI160_GYRO_RANGE_500,      /**<  +/-  500 degrees/second */
+        BMI160_GYRO_RANGE_250,      /**<  +/-  250 degrees/second */
+        BMI160_GYRO_RANGE_125,      /**<  +/-  125 degrees/second */
+    };
+
+    enum {
+        BMI160_ACCEL_RATE_25_2HZ = 5,  /**<   25/2  Hz */
+        BMI160_ACCEL_RATE_25HZ,        /**<   25    Hz */
+        BMI160_ACCEL_RATE_50HZ,        /**<   50    Hz */
+        BMI160_ACCEL_RATE_100HZ,       /**<  100    Hz */
+        BMI160_ACCEL_RATE_200HZ,       /**<  200    Hz */
+        BMI160_ACCEL_RATE_400HZ,       /**<  400    Hz */
+        BMI160_ACCEL_RATE_800HZ,       /**<  800    Hz */
+        BMI160_ACCEL_RATE_1600HZ,      /**< 1600    Hz */
+    };
+
+    enum {
+        BMI160_GYRO_RATE_25HZ = 6,     /**<   25    Hz */
+        BMI160_GYRO_RATE_50HZ,         /**<   50    Hz */
+        BMI160_GYRO_RATE_100HZ,        /**<  100    Hz */
+        BMI160_GYRO_RATE_200HZ,        /**<  200    Hz */
+        BMI160_GYRO_RATE_400HZ,        /**<  400    Hz */
+        BMI160_GYRO_RATE_800HZ,        /**<  800    Hz */
+        BMI160_GYRO_RATE_1600HZ,       /**< 1600    Hz */
+        BMI160_GYRO_RATE_3200HZ,       /**< 3200    Hz */
+    };
+
+    int begin(BusDevice * bus) override
+    {
+      return begin(bus, BMI160_ADDRESS_FIRST) ? 1 : begin(bus, BMI160_ADDRESS_SECOND) ? 1 : 0;
+    }
+
+    int begin(BusDevice * bus, uint8_t addr) override
+    {
+      setBus(bus, addr);
+
+      if(!testConnection()) return 0;
+
+      // reset device
+      _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_SOFT_RESET);
+      delay(1);
+
+      if(_bus->isSPI())
+      {
+        /* 
+          Issue a dummy-read to force the device into SPI comms mode 
+          see https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi160-ds000.pdf section 3.2.1
+        */
+        uint8_t dummy = 0;
+        _bus->readByte(_addr, BMI160_CMD_SPI_MODE, &dummy);
+        delay(100);
+      }
+
+      // Start up accelerometer
+      _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL);
+      delay(100);
+
+      // Start up gyroscope
+      _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL);
+      delay(100);
+
+      // Set up full scale Accel range. +-16G
+      _bus->writeByte(_addr, BMI160_RA_ACCEL_RANGE, BMI160_ACCEL_RANGE_16G);
+      delay(1);
+
+      // Set up full scale Gyro range. +-2000dps
+      _bus->writeByte(_addr, BMI160_RA_GYRO_RANGE, BMI160_GYRO_RANGE_2000);
+      delay(1);
+
+      // Enable accel offset
+      _bus->writeBitsBMI160(_addr, BMI160_RA_OFFSET_6, BMI160_ACC_OFFSET_EN, BMI160_ACC_OFFSET_LEN, BMI160_RESULT_OK);
+      delay(1);
+
+      // Enable gyro offset
+      _bus->writeBitsBMI160(_addr, BMI160_RA_OFFSET_6, BMI160_GYR_OFFSET_EN, BMI160_GYR_OFFSET_LEN, BMI160_RESULT_OK);
+      delay(1);
+
+      // Enable data ready interrupt
+      _bus->writeByte(_addr, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
+      delay(1);
+
+      // Enable INT1 pin
+      _bus->writeByte(_addr, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
+      delay(1);
+
+      // Map data ready interrupt to INT1 pin
+      _bus->writeByte(_addr, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
+      delay(1);
+
+      // Set Accel rate 1600HZ
+      _bus->writeByte(_addr, BMI160_RA_ACCEL_CONF, BMI160_ACCEL_RATE_1600HZ);
+      delay(1);
+
+      // Set Gyro rate 3200HZ
+      _bus->writeByte(_addr, BMI160_RA_GYRO_CONF, BMI160_GYRO_RATE_3200HZ);
+      delay(1);
+
+      return 1;
+    }
+
+    GyroDeviceType getType() const override
+    {
+      return GYRO_BMI160;
+    }
+
+    int readGyro(VectorInt16& v) override
+    {
+      uint8_t buffer[6];
+
+      _bus->readFast(_addr, BMI160_RA_GYRO_X_L, 6, buffer);
+
+      v.x = (((int16_t)buffer[1]) << 8) | buffer[0];
+      v.y = (((int16_t)buffer[3]) << 8) | buffer[2];
+      v.z = (((int16_t)buffer[5]) << 8) | buffer[4];
+
+      return 1;
+    }
+
+    int readAccel(VectorInt16& v) override
+    {
+      uint8_t buffer[6];
+
+      _bus->readFast(_addr, BMI160_RA_ACCEL_X_L, 6, buffer);
+
+      v.x = (((int16_t)buffer[1]) << 8) | buffer[0];
+      v.y = (((int16_t)buffer[3]) << 8) | buffer[2];
+      v.z = (((int16_t)buffer[5]) << 8) | buffer[4];
+
+      return 1;
+    }
+
+    void setDLPFMode(uint8_t mode) override
+    {
+    }
+
+    int getRate() const override
+    {
+      uint8_t valRate = 0;
+      _bus->readBitsBMI160(_addr, BMI160_RA_GYRO_CONF, BMI160_GYRO_RATE_SEL_BIT, BMI160_GYRO_RATE_SEL_LEN, &valRate);
+
+      switch (valRate)
+      {
+      case BMI160_GYRO_RATE_3200HZ:
+        return 3200;
+        break;
+      case BMI160_GYRO_RATE_1600HZ:
+        return 1600;
+        break;
+      case BMI160_GYRO_RATE_800HZ:
+        return 800;
+        break;
+      case BMI160_GYRO_RATE_400HZ:
+        return 400;
+        break;
+      case BMI160_GYRO_RATE_200HZ:
+        return 200;
+        break;
+      case BMI160_GYRO_RATE_100HZ:
+        return 100;
+        break;
+      case BMI160_GYRO_RATE_50HZ:
+        return 50;
+        break;
+      case BMI160_GYRO_RATE_25HZ:
+        return 25;
+        break;
+      default:
+        return 99; //detect error
+      }
+    }
+
+    void setRate(int rate) override
+    {
+    }
+
+    void setFullScaleGyroRange(uint8_t range) override
+    {
+    }
+
+    void setFullScaleAccelRange(uint8_t range) override
+    {
+    }
+
+    bool testConnection() override
+    {
+      uint8_t whoami = 0;
+      _bus->readByte(_addr, BMI160_RA_CHIP_ID, &whoami);
+      //D("bmi160:whoami", _addr, whoami);
+      return whoami == BMI160_CHIP_ID_DEFAULT_VALUE;
+    }
+
+    void setSleepEnabled(bool enabled)
+    {
+    }
+
+    void setClockSource(uint8_t source)
+    {
+    }
+};
+
+}
+
+}
+
+#endif

lib/Espfc/src/Device/GyroDevice.h

@@ -16,6 +16,7 @@ enum GyroDeviceType {
   GYRO_MPU9250 = 5,
   GYRO_LSM6DSO = 6,
   GYRO_ICM20602 = 7,
+  GYRO_BMI160 = 8,
   GYRO_MAX
 };
 
@@ -44,7 +45,7 @@ class GyroDevice: public BusAwareDevice
 
     static const char ** getNames()
     {
-      static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"), NULL };
+      static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"),PSTR("BMI160"), NULL };
       return devChoices;
     }