From 0eb6ae365ab688f8128b4d41a7cf1743287b698b Mon Sep 17 00:00:00 2001
From: Leonardo Garcia <lgarciaos@protonmail.com>
Date: Tue, 22 Mar 2022 02:22:17 -0700
Subject: [PATCH] AP_InertialSensor: Added BMI085 support

Fixed styling for 61125e2

Remove whitespace l202

Removed else
---
 Tools/scripts/decode_devid.py                 |  1 +
 .../AP_InertialSensor_BMI088.cpp              | 31 +++++++++++++------
 .../AP_InertialSensor_BMI088.h                |  1 +
 .../AP_InertialSensor_Backend.h               |  1 +
 4 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py
index 43fc0cdc9d..ace2b97e4c 100755
--- a/Tools/scripts/decode_devid.py
+++ b/Tools/scripts/decode_devid.py
@@ -97,6 +97,7 @@ imu_types = {
     0x36 : "DEVTYPE_INS_ICM40605",
     0x37 : "DEVTYPE_INS_IIM42652",
     0x38 : "DEVTYPE_INS_BMI270",
+    0x39 : "DEVTYPE_INS_BMI085",
 }
 
 baro_types = {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
index 11afa0bbec..60f67d41a1 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
@@ -28,8 +28,8 @@
 #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_TEMP_MSB      0x22
+#define REGA_TEMP_LSB      0x23
 #define REGA_CONF          0x40
 #define REGA_RANGE         0x41
 #define REGA_PWR_CONF      0x7C
@@ -97,7 +97,7 @@ AP_InertialSensor_BMI088::probe(AP_InertialSensor &imu,
 
 void AP_InertialSensor_BMI088::start()
 {
-    if (!_imu.register_accel(accel_instance, ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088)) ||
+    if (!_imu.register_accel(accel_instance, ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(_accel_devtype)) ||
         !_imu.register_gyro(gyro_instance, GYRO_BACKEND_SAMPLE_RATE,   dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088))) {
         return;
     }
@@ -161,7 +161,7 @@ static const struct {
     { REGA_PWR_CONF, 0 },
     { REGA_PWR_CTRL, 0x04 },
     // setup FIFO for streaming X,Y,Z
-    { REGA_FIFO_CONFIG0, 0x00 },
+    { REGA_FIFO_CONFIG0, 0x02 },
     { REGA_FIFO_CONFIG1, 0x50 },
 };
 
@@ -200,15 +200,28 @@ bool AP_InertialSensor_BMI088::accel_init()
     // dummy ready on accel ChipID to init accel (see section 3 of datasheet)
     read_accel_registers(REGA_CHIPID, &v, 1);
 
-    if (!read_accel_registers(REGA_CHIPID, &v, 1) || v != 0x1E) {
+    if (!read_accel_registers(REGA_CHIPID, &v, 1)) {
         return false;
     }
 
-    if (!setup_accel_config()) {
-        hal.console->printf("BMI088: delaying accel config\n");
+    switch (v) {
+        case 0x1E:
+            _accel_devtype = DEVTYPE_INS_BMI088;
+            hal.console->printf("BMI088: Found device\n");
+            break;
+        case 0x1F:
+            _accel_devtype = DEVTYPE_INS_BMI085;
+            hal.console->printf("BMI085: Found device\n");
+            break;
+        default:
+            return false;
     }
 
-    hal.console->printf("BMI088: found accel\n");
+    if (!setup_accel_config()) {
+        hal.console->printf("BMI08x: delaying accel config\n");
+    }
+
+    hal.console->printf("BMI08x: found accel\n");
 
     return true;
 }
@@ -353,7 +366,7 @@ void AP_InertialSensor_BMI088::read_fifo_accel(void)
     if (temperature_counter++ == 100) {
         temperature_counter = 0;
         uint8_t tbuf[2];
-        if (!read_accel_registers(REGA_TEMP_LSB, tbuf, 2)) {
+        if (!read_accel_registers(REGA_TEMP_MSB, tbuf, 2)) {
             _inc_accel_error_count(accel_instance);
         } else {
             uint16_t temp_uint11 = (tbuf[0]<<3) | (tbuf[1]>>5);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
index cf1abd775f..fbb60b5760 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
@@ -81,6 +81,7 @@ private:
     uint8_t gyro_instance;
     enum Rotation rotation;
     uint8_t temperature_counter;
+    enum DevTypes _accel_devtype;
 
     bool done_accel_config;
     uint32_t accel_config_count;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index 2dcd56958b..47dc1854d8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -121,6 +121,7 @@ public:
         DEVTYPE_INS_ICM40605 = 0x36,
         DEVTYPE_INS_IIM42652 = 0x37,
         DEVTYPE_BMI270       = 0x38,
+        DEVTYPE_INS_BMI085   = 0x39,
     };
 
 protected: