[IMU Driver]: Add CRT to BMI270 (#31)

* add crt for bmi270

* remove 6500 to fix cli lock issue

src/main/drivers/accgyro/accgyro_spi_bmi270.c

@@ -76,7 +76,9 @@ typedef enum {
     BMI270_REG_FIFO_LENGTH_MSB = 0x25,
     BMI270_REG_FIFO_DATA = 0x26,
     BMI270_REG_FEAT_PAGE = 0x2F,
+    BMI270_REG_FEATURES_0_GYR_GAIN_STATUS = 0x38,
     BMI270_REG_FEATURES_0_GYR_CAS = 0x3C,
+    BMI270_REG_FEATURES_1_G_TRIG_1 = 0x32,
     BMI270_REG_FEATURES_1_GEN_SET_1 = 0x34,
     BMI270_REG_ACC_CONF = 0x40,
     BMI270_REG_ACC_RANGE = 0x41,
@@ -97,6 +99,7 @@ typedef enum {
     BMI270_REG_INT_MAP_DATA = 0x58,
     BMI270_REG_INIT_CTRL = 0x59,
     BMI270_REG_INIT_DATA = 0x5E,
+    BMI270_REG_GYR_CRT_CONF = 0x69,
     BMI270_REG_IF_CONF = 0x6B,
     BMI270_REG_ACC_SELF_TEST = 0x6D,
     BMI270_REG_GYR_SELF_TEST_AXES = 0x6E,
@@ -108,10 +111,15 @@ typedef enum {
 
 // BMI270 register configuration values
 typedef enum {
+    BMI270_VAL_CMD_G_TRIGGER = 0x02,
     BMI270_VAL_CMD_SOFTRESET = 0xB6,
     BMI270_VAL_CMD_FIFOFLUSH = 0xB0,
     BMI270_VAL_PWR_CTRL_DISABLE_ALL = 0x00,             // disable all sensors
     BMI270_VAL_PWR_CTRL = 0x0E,                         // enable gyro, acc and temp sensors
+    BMI270_VAL_PWR_CTRL_ACC_ENABLE = BIT(2),            // bit 2, enable acc
+    BMI270_VAL_PWR_CTRL_ACC_DISABLE = 0x00,             // bit 2, disable gyro
+    BMI270_VAL_PWR_CONF_ADV_POWER_SAVE_DISABLE = 0x00,  // disable ADV PS
+    BMI270_VAL_PWR_CONF_ADV_POWER_SAVE_ENABLE = BIT(0), // enable ADV PS
     BMI270_VAL_PWR_CONF = 0x02,                         // disable advanced power save, enable FIFO self-wake
     BMI270_VAL_PAGE_0 = 0x00,                           // select page 0
     BMI270_VAL_PAGE_1 = 0x01,                           // select page 1
@@ -143,11 +151,24 @@ typedef enum {
     BMI270_VAL_FIFO_WTM_1 = 0x00,                       // FIFO watermark MSB
     BMI270_VAL_GEN_SET_1 = 0x0200,                      // bit 9, enable self offset correction (IOC part 1)
     BMI270_VAL_OFFSET_6 = 0xC0,                         // Enable sensitivity error compensation and gyro offset compensation (IOC part2)
+    BMI270_VAL_OFFSET_6_GYR_GAIN_EN_ENABLE = BIT(7),    // bit 7, enable gyro gain compensation
+    BMI270_VAL_GYR_CRT_CONF_CRT_RUNNING = BIT(2),       // bit 7, enable gyro crt
+    BMI270_VAL_FEATURES_1_G_TRIG_1_SELECT_CRT = 0x0100, // bit 8, CRT will be executed
+    BMI270_VAL_FEATURES_1_G_TRIG_1_SELECT_GYR_BIST=0x0000, // bit 8, gyro built-in self-test will be executed
+    BMI270_VAL_FEATURES_1_G_TRIG_1_BLOCK_UNLOCK = 0x0000,// bit 9, do not block further G_TRIGGER commands
+    BMI270_VAL_FEATURES_1_G_TRIG_1_BLOCK_BLOCK = 0x0200, // bit 9, block further G_TRIGGER commands
 } bmi270ConfigValues_e;
 
 typedef enum {
     BMI270_MASK_FEATURES_1_GEN_SET_1 = 0x0200,
     BMI270_MASK_OFFSET_6 = 0xC0,
+    BMI270_MASK_OFFSET_6_GYR_GAIN_ENABLE = 0x80,        // bit 7, enable gyro gain compensation
+    BMI270_MASK_PWR_ADV_POWER_SAVE = 0x01,              // bit 0, enable advanced power save
+    BMI270_MASK_PWR_CTRL_ACC_ENABLE = 0x04,             // bit 2, enable acc
+    BMI270_MASK_GYR_CRT_CONF_CRT_RUNNING = 0x04,        // bit 2, gyro CRT running
+    BMI270_MASK_G_TRIG_1_SELECT = 0x0100,               // bit 8, select feature that should be executed
+    BMI270_MASK_G_TRIG_1_BLOCK = 0x0200,                // bit 9, block feature with next G_TRIGGER CMD
+    BMI270_MASK_GYR_GAIN_STATUS_G_TRIG_STATUS = 0x38    // bit[5:3]
 } bmi270ConfigMasks_e;
 
 // Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
@@ -196,7 +217,7 @@ static void bmi270RegisterWriteBits(const extDevice_t *dev, bmi270Register_e reg
 static void bmi270RegisterWrite16(const extDevice_t *dev, bmi270Register_e registerId, uint16_t data, unsigned delayMs)
 {
     uint8_t buf[2] = {
-        (uint8_t)(data & 0x00FF),   // LSB first since address is auto-incremented
+        (uint8_t)(data & 0x00FF),
         (uint8_t)(data >> 8)
     };
     spiWriteRegBuf(dev, registerId, buf, 2);
@@ -246,6 +267,71 @@ static void bmi270UploadConfig(const extDevice_t *dev)
     bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1);
 }
 
+static void bmi270EnableIOC(const extDevice_t *dev)
+{
+    // Configure the feature register page to page 1
+    bmi270RegisterWrite(dev, BMI270_REG_FEAT_PAGE, BMI270_VAL_PAGE_1, 1);
+
+    // Enable self offset correction
+    bmi270RegisterWriteBits16(dev, BMI270_REG_FEATURES_1_GEN_SET_1, BMI270_MASK_FEATURES_1_GEN_SET_1, BMI270_VAL_GEN_SET_1, 1);
+
+    // Enable IOC
+    bmi270RegisterWriteBits(dev, BMI270_REG_OFFSET_6, BMI270_MASK_OFFSET_6, 0x40, 1);
+
+    // Configure the feature register page to page 0
+    bmi270RegisterWrite(dev, BMI270_REG_FEAT_PAGE, BMI270_VAL_PAGE_0, 1);
+}
+
+static void bmi270PerformCRT(const extDevice_t *dev)
+{
+    // disable advance power save mode
+    bmi270RegisterWriteBits(dev, BMI270_REG_PWR_CONF, BMI270_MASK_PWR_ADV_POWER_SAVE, BMI270_VAL_PWR_CONF_ADV_POWER_SAVE_DISABLE, 10);
+
+    // enable gyro gain compensation, set OFFSET_6.gain_comp_en = 1
+    bmi270RegisterWriteBits(dev, BMI270_REG_OFFSET_6, BMI270_MASK_OFFSET_6_GYR_GAIN_ENABLE, BMI270_VAL_OFFSET_6_GYR_GAIN_EN_ENABLE, 10);
+
+    // enable acc, set pwr_ctrl.acc_en = 1
+    bmi270RegisterWriteBits(dev, BMI270_REG_PWR_CTRL, BMI270_MASK_PWR_CTRL_ACC_ENABLE, BMI270_VAL_PWR_CTRL_ACC_ENABLE, 10);
+
+    // set GYR_CRT_CONF.crt_running = 0b1
+    bmi270RegisterWriteBits(dev, BMI270_REG_GYR_CRT_CONF, BMI270_MASK_GYR_CRT_CONF_CRT_RUNNING, BMI270_VAL_GYR_CRT_CONF_CRT_RUNNING, 10);
+
+    // set page to 1
+    bmi270RegisterWrite(dev, BMI270_REG_FEAT_PAGE, BMI270_VAL_PAGE_1, 10);
+
+    // set G_TRIG_1.select =1
+    bmi270RegisterWriteBits16(dev, BMI270_REG_FEATURES_1_G_TRIG_1, BMI270_MASK_G_TRIG_1_SELECT, BMI270_VAL_FEATURES_1_G_TRIG_1_SELECT_CRT, 10);
+
+    // set G_TRIG_1.block = 0
+    // bmi270RegisterWriteBits16(dev, BMI270_REG_FEATURES_1_G_TRIG_1, BMI270_MASK_G_TRIG_1_BLOCK, BMI270_VAL_FEATURES_1_G_TRIG_1_BLOCK_UNLOCK, 1);
+
+    // send g_trigger command to CMD reg
+    bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_G_TRIGGER, 5000);
+
+    // read GYR_CRT_CONF.crt_running and check if it is 0
+    uint8_t bmiCheck = bmi270RegisterRead(dev, BMI270_REG_GYR_CRT_CONF);
+    // if not, wait for 2 seconds and check again max time for 5
+    uint8_t checkCount = 0;
+
+    while ((bmiCheck!=0x00) && (checkCount++ < 5)) {
+        bmiCheck = bmi270RegisterRead(dev, BMI270_REG_GYR_CRT_CONF);
+        delay(1000);
+        if (bmiCheck == 0x00) {
+            break;
+        }
+    }
+
+    delay(100);
+    // set page to 0
+    bmi270RegisterWrite(dev, BMI270_REG_FEAT_PAGE, BMI270_VAL_PAGE_0, 1);
+
+    // read GYR_GAIN_STATUS.g_trig_status
+    uint16_t bmiStatus = bmi270RegisterRead16(dev, BMI270_REG_FEATURES_0_GYR_GAIN_STATUS);
+    if (bmiStatus == 0x0000){
+        delay(10);
+    }
+}
+
 static uint8_t getBmiOsrMode()
 {
     switch(gyroConfig()->gyro_hardware_lpf) {
@@ -282,6 +368,8 @@ static void bmi270Config(gyroDev_t *gyro)
 
     bmi270UploadConfig(dev);
 
+    bmi270PerformCRT(dev);
+
     // Disable all sensors
     bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL_DISABLE_ALL, 1);
 
@@ -328,17 +416,8 @@ static void bmi270Config(gyroDev_t *gyro)
     // Enable the gyro, accelerometer and temperature sensor - disable aux interface
     bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1);
 
-    // Configure the feature register page to page 1
-    bmi270RegisterWrite(dev, BMI270_REG_FEAT_PAGE, BMI270_VAL_PAGE_1, 1);
-
-    // Enable self offset correction
-    bmi270RegisterWriteBits16(dev, BMI270_REG_FEATURES_1_GEN_SET_1, BMI270_MASK_FEATURES_1_GEN_SET_1, BMI270_VAL_GEN_SET_1, 1);
-
     // Enable IOC
-    bmi270RegisterWriteBits(dev, BMI270_REG_OFFSET_6, BMI270_MASK_OFFSET_6, 0x40, 1);
+    bmi270EnableIOC(dev);
-
-    // Configure the feature register page to page 0
-    bmi270RegisterWrite(dev, BMI270_REG_FEAT_PAGE, BMI270_VAL_PAGE_0, 1);
 
     // Flush the FIFO
     if (fifoMode) {

src/main/target/NEUTRONRCF435MINI/target.h

@@ -86,8 +86,8 @@
 
 
 #define USE_GYRO
-#define USE_GYRO_SPI_MPU6500  //debug only
+// #define USE_GYRO_SPI_MPU6500  //debug only
-#define USE_ACC_SPI_MPU6500		//debug only
+// #define USE_ACC_SPI_MPU6500		//debug only
 #define USE_GYRO_SPI_ICM42688P
 #define USE_ACCGYRO_BMI270
 #define USE_ACCGYRO_LSM6DSO

src/main/target/NEUTRONRCF435MINI/target.mk

@@ -5,9 +5,7 @@ CUSTOM_DEFAULTS_EXTENDED = yes
 FEATURES       +=  VCP ONBOARDFLASH
 
 TARGET_SRC = \
-        drivers/accgyro/accgyro_mpu6500.c \
         drivers/accgyro/accgyro_spi_mpu6000.c \
-        drivers/accgyro/accgyro_spi_mpu6500.c\
         drivers/accgyro/accgyro_spi_icm426xx.c\
         drivers/barometer/barometer_bmp280.c \
         drivers/barometer/barometer_dps310.c\

src/main/target/NEUTRONRCF435SE/target.h

@@ -89,8 +89,8 @@
 
 
 #define USE_GYRO
-#define USE_GYRO_SPI_MPU6500  //debug only
+// #define USE_GYRO_SPI_MPU6500  //debug only
-#define USE_ACC_SPI_MPU6500		//debug only
+// #define USE_ACC_SPI_MPU6500		//debug only
 #define USE_GYRO_SPI_ICM42688P
 #define USE_ACCGYRO_BMI270
 #define USE_ACCGYRO_LSM6DSL

src/main/target/NEUTRONRCF435SE/target.mk

@@ -5,9 +5,7 @@ CUSTOM_DEFAULTS_EXTENDED = yes
 FEATURES       +=  VCP ONBOARDFLASH
 
 TARGET_SRC = \
-			drivers/accgyro/accgyro_mpu6500.c \
             drivers/accgyro/accgyro_spi_mpu6000.c \
-            drivers/accgyro/accgyro_spi_mpu6500.c\
             drivers/accgyro/accgyro_spi_icm426xx.c\
             drivers/barometer/barometer_bmp280.c \
             drivers/barometer/barometer_dps310.c\