Add driver for BMI088 IMU

src/main/CMakeLists.txt

@@ -71,6 +71,8 @@ main_sources(COMMON_SRC
     drivers/accgyro/accgyro_adxl345.h
     drivers/accgyro/accgyro_bma280.c
     drivers/accgyro/accgyro_bma280.h
+    drivers/accgyro/accgyro_bmi088.c
+    drivers/accgyro/accgyro_bmi088.h
     drivers/accgyro/accgyro_bmi160.c
     drivers/accgyro/accgyro_bmi160.h
     drivers/accgyro/accgyro_fake.c

src/main/drivers/accgyro/accgyro_bmi088.c

@@ -0,0 +1,245 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/system.h"
+#include "drivers/time.h"
+#include "drivers/io.h"
+#include "drivers/exti.h"
+#include "drivers/bus.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_bmi088.h"
+
+
+#if defined(USE_IMU_BMI088)
+
+/*
+  device registers, names follow datasheet conventions, with REGA_
+  prefix for accel, and REGG_ prefix for gyro
+ */
+#define REGA_CHIPID        0x00
+#define REGA_ERR_REG       0x02
+#define REGA_STATUS        0x03
+#define REGA_X_LSB         0x12
+#define REGA_INT_STATUS_1  0x1D
+#define REGA_TEMP_LSB      0x22
+#define REGA_TEMP_MSB      0x23
+#define REGA_CONF          0x40
+#define REGA_RANGE         0x41
+#define REGA_PWR_CONF      0x7C
+#define REGA_PWR_CTRL      0x7D
+#define REGA_SOFTRESET     0x7E
+#define REGA_FIFO_CONFIG0  0x48
+#define REGA_FIFO_CONFIG1  0x49
+#define REGA_FIFO_DOWNS    0x45
+#define REGA_FIFO_DATA     0x26
+#define REGA_FIFO_LEN0     0x24
+#define REGA_FIFO_LEN1     0x25
+
+#define REGG_CHIPID        0x00
+#define REGG_RATE_X_LSB    0x02
+#define REGG_INT_CTRL      0x15
+#define REGG_INT_STATUS_1  0x0A
+#define REGG_INT_STATUS_2  0x0B
+#define REGG_INT_STATUS_3  0x0C
+#define REGG_FIFO_STATUS   0x0E
+#define REGG_RANGE         0x0F
+#define REGG_BW            0x10
+#define REGG_LPM1          0x11
+#define REGG_RATE_HBW      0x13
+#define REGG_BGW_SOFTRESET 0x14
+#define REGG_FIFO_CONFIG_1 0x3E
+#define REGG_FIFO_DATA     0x3F
+
+
+static void bmi088GyroInit(gyroDev_t *gyro)
+{
+    gyroIntExtiInit(gyro);
+
+    busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
+
+    // Soft reset
+    busWrite(gyro->busDev, REGG_BGW_SOFTRESET, 0xB6);
+    delay(100);
+
+    // ODR 2kHz, BW 532Hz
+    busWrite(gyro->busDev, REGG_BW, 0x81);
+    delay(1);
+
+    // Enable sampling
+    busWrite(gyro->busDev, REGG_INT_CTRL, 0x80);
+    delay(1);
+
+    busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
+}
+
+static void bmi088AccInit(accDev_t *acc)
+{
+    busSetSpeed(acc->busDev, BUS_SPEED_INITIALIZATION);
+
+    // Soft reset
+    busWrite(acc->busDev, REGA_SOFTRESET, 0xB6);
+    delay(100);
+
+    // Active mode
+    busWrite(acc->busDev, REGA_PWR_CONF, 0);
+    delay(100);
+
+    // ACC ON
+    busWrite(acc->busDev, REGA_PWR_CTRL, 0x04);
+    delay(100);
+
+    // OSR4, ODR 1600Hz
+    busWrite(acc->busDev, REGA_CONF, 0x8C);
+    delay(1);
+
+    // Range 12g
+    busWrite(acc->busDev, REGA_RANGE, 0x02);
+    delay(1);
+
+    busSetSpeed(acc->busDev, BUS_SPEED_STANDARD);
+
+    acc->acc_1G = 2048;
+}
+
+static bool bmi088GyroRead(gyroDev_t *gyro)
+{
+    uint8_t gyroRaw[6];
+
+    if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) {
+        gyro->gyroADCRaw[X] = (int16_t)((gyroRaw[1] << 8) | gyroRaw[0]);
+        gyro->gyroADCRaw[Y] = (int16_t)((gyroRaw[3] << 8) | gyroRaw[2]);
+        gyro->gyroADCRaw[Z] = (int16_t)((gyroRaw[5] << 8) | gyroRaw[4]);
+        return true;
+    }
+
+    return false;
+}
+
+static bool bmi088AccRead(accDev_t *acc)
+{
+    uint8_t buffer[7];
+
+    if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) {
+        // first byte is discarded, see datasheet
+        acc->ADCRaw[X] = (int32_t)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
+        acc->ADCRaw[Y] = (int32_t)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
+        acc->ADCRaw[Z] = (int32_t)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
+        return true;
+    }
+
+    return false;
+}
+
+static bool gyroDeviceDetect(busDevice_t * busDev)
+{
+    uint8_t attempts;
+
+    busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
+
+    for (attempts = 0; attempts < 5; attempts++) {
+        uint8_t chipId;
+
+        delay(100);
+        busRead(busDev, REGG_CHIPID, &chipId);
+
+        if (chipId == 0x0F) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+static bool accDeviceDetect(busDevice_t * busDev)
+{
+    uint8_t attempts;
+
+    busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
+
+    for (attempts = 0; attempts < 5; attempts++) {
+        uint8_t chipId;
+
+        delay(100);
+        busRead(busDev, REGA_CHIPID, &chipId);
+
+        if (chipId == 0x1E) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool bmi088AccDetect(accDev_t *acc)
+{
+    acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_ACC, acc->imuSensorToUse, OWNER_MPU);
+    if (acc->busDev == NULL) {
+        return false;
+    }
+
+    if (!accDeviceDetect(acc->busDev)) {
+        busDeviceDeInit(acc->busDev);
+        return false;
+    }
+
+    acc->initFn = bmi088AccInit;
+    acc->readFn = bmi088AccRead;
+
+    return true;
+}
+
+bool bmi088GyroDetect(gyroDev_t *gyro)
+{
+    gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_GYRO, gyro->imuSensorToUse, OWNER_MPU);
+    if (gyro->busDev == NULL) {
+        return false;
+    }
+
+    if (!gyroDeviceDetect(gyro->busDev)) {
+        busDeviceDeInit(gyro->busDev);
+        return false;
+    }
+
+    gyro->initFn = bmi088GyroInit;
+    gyro->readFn = bmi088GyroRead;
+    gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb
+    gyro->intStatusFn = gyroCheckDataReady;
+    gyro->gyroAlign = gyro->busDev->param;
+
+    return true;
+}
+
+#endif /* USE_IMU_BMI088 */

src/main/drivers/accgyro/accgyro_bmi088.h

@@ -0,0 +1,30 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "drivers/sensor.h"
+
+bool bmi088AccDetect(accDev_t *acc);
+bool bmi088GyroDetect(gyroDev_t *gyro);

src/main/drivers/bus.h

@@ -91,6 +91,8 @@ typedef enum {
     DEVHW_MPU6050,
     DEVHW_MPU6500,
     DEVHW_BMI160,
+    DEVHW_BMI088_GYRO,
+    DEVHW_BMI088_ACC,
     DEVHW_ICM20689,
 
     /* Combined ACC/GYRO/MAG chips */

src/main/fc/settings.yaml

@@ -4,7 +4,7 @@ tables:
   - name: gyro_lpf
     values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"]
   - name: acc_hardware
-    values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"]
+    values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"]

src/main/sensors/acceleration.c

@@ -50,6 +50,7 @@ FILE_COMPILE_FOR_SPEED
 #include "drivers/accgyro/accgyro_adxl345.h"
 #include "drivers/accgyro/accgyro_mma845x.h"
 #include "drivers/accgyro/accgyro_bma280.h"
+#include "drivers/accgyro/accgyro_bmi088.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_fake.h"
@@ -240,6 +241,19 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
+#if defined(USE_IMU_BMI088)
+    case ACC_BMI088:
+        if (bmi088AccDetect(dev)) {
+            accHardware = ACC_BMI088;
+            break;
+        }
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (accHardwareToUse != ACC_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_IMU_ICM20689
     case ACC_ICM20689:
         if (icm20689AccDetect(dev)) {

src/main/sensors/acceleration.h

@@ -45,7 +45,8 @@ typedef enum {
     ACC_MPU9250 = 9,
     ACC_BMI160 = 10,
     ACC_ICM20689 = 11,
-    ACC_FAKE = 12,
+    ACC_BMI088 = 12,
+    ACC_FAKE = 13,
     ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 

src/main/sensors/gyro.c

@@ -52,6 +52,7 @@ FILE_COMPILE_FOR_SPEED
 #include "drivers/accgyro/accgyro_adxl345.h"
 #include "drivers/accgyro/accgyro_mma845x.h"
 #include "drivers/accgyro/accgyro_bma280.h"
+#include "drivers/accgyro/accgyro_bmi088.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_fake.h"
@@ -202,6 +203,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
+#ifdef USE_IMU_BMI088
+    case GYRO_BMI088:
+        if (bmi088GyroDetect(dev)) {
+            gyroHardware = GYRO_BMI088;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_IMU_ICM20689
     case GYRO_ICM20689:
         if (icm20689GyroDetect(dev)) {

src/main/sensors/gyro.h

@@ -36,6 +36,7 @@ typedef enum {
     GYRO_MPU9250,
     GYRO_BMI160,
     GYRO_ICM20689,
+    GYRO_BMI088,
     GYRO_FAKE
 } gyroSensor_e;
 

src/main/target/common_hardware.c

@@ -80,6 +80,15 @@
         BUSDEV_REGISTER_I2C(busdev_bmi160,      DEVHW_BMI160,       BMI160_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI160_ALIGN);
         #endif
     #endif
+
+    #if defined(USE_IMU_BMI088)
+        #if defined(BMI088_SPI_BUS)
+        BUSDEV_REGISTER_SPI(busdev_bmi088_gyro, DEVHW_BMI088_GYRO,  BMI088_SPI_BUS,     BMI088_GYRO_CS_PIN, GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI088_ALIGN);
+        BUSDEV_REGISTER_SPI(busdev_bmi088_acc,  DEVHW_BMI088_ACC,   BMI088_SPI_BUS,     BMI088_ACC_CS_PIN,  GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI088_ALIGN);
+        #elif defined(BMI088_I2C_BUS)
+        BUSDEV_REGISTER_I2C(busdev_bmi088,      DEVHW_BMI088,       BMI088_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI088_ALIGN);
+        #endif
+    #endif
 #endif