From 0c2a1bf0795586c05186feaed8869cc9c9fc4a6b Mon Sep 17 00:00:00 2001
From: Leonardo Garcia <lgarciaos@protonmail.com>
Date: Thu, 13 Oct 2022 16:50:39 -0700
Subject: [PATCH] AP_InertialSensor: Fix BMI085 accel scaling

Original BMI088 has 24G range so it was hardcoded for *scale* within
read_fifo_accel. Added a class variable accel_range which is assigned
the correct value when the sensor type is received (16.0 or 24.0).
---
 libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp | 8 +++++---
 libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h   | 1 +
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
index b71420f3e143e..9188e99fb90d5 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
@@ -155,7 +155,7 @@ static const struct {
     uint8_t value;
 } accel_config[] = {
     { REGA_CONF, 0xAC },
-    // setup 24g range
+    // setup 24g range (16g for BMI085)
     { REGA_RANGE, 0x03 },
     // disable low-power mode
     { REGA_PWR_CONF, 0 },
@@ -207,10 +207,12 @@ bool AP_InertialSensor_BMI088::accel_init()
     switch (v) {
         case 0x1E:
             _accel_devtype = DEVTYPE_INS_BMI088;
+            accel_range = 24.0;
             hal.console->printf("BMI088: Found device\n");
             break;
         case 0x1F:
             _accel_devtype = DEVTYPE_INS_BMI085;
+            accel_range = 16.0;
             hal.console->printf("BMI085: Found device\n");
             break;
         default:
@@ -317,8 +319,8 @@ void AP_InertialSensor_BMI088::read_fifo_accel(void)
         _inc_accel_error_count(accel_instance);
         return;
     }
-    // assume configured for 24g range
-    const float scale = (1.0/32768.0) * GRAVITY_MSS * 24.0;
+    // use new accel_range depending on sensor type
+    const float scale = (1.0/32768.0) * GRAVITY_MSS * accel_range;
     const uint8_t *p = &data[0];
     while (fifo_length >= 7) {
         /*
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
index fbb60b5760618..51b2cdcabc97d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
@@ -82,6 +82,7 @@ class AP_InertialSensor_BMI088 : public AP_InertialSensor_Backend {
     enum Rotation rotation;
     uint8_t temperature_counter;
     enum DevTypes _accel_devtype;
+    float accel_range;
 
     bool done_accel_config;
     uint32_t accel_config_count;