From fa63243eb04671564fc69f325981e3e03c130087 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 15 Mar 2019 14:32:41 +1100
Subject: [PATCH] AP_InertialSensor: added BMI088 accel config retry

---
 .../AP_InertialSensor_BMI088.cpp              | 67 ++++++++++++-------
 .../AP_InertialSensor_BMI088.h                |  8 +++
 2 files changed, 51 insertions(+), 24 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
index 8bf807e1d7..601dd97698 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
@@ -146,6 +146,44 @@ bool AP_InertialSensor_BMI088::write_accel_register(uint8_t reg, uint8_t v)
     return false;
 }
 
+static const struct {
+    uint8_t reg;
+    uint8_t value;
+} accel_config[] = {
+    { REGA_CONF, 0xAC },
+    // setup 24g range
+    { REGA_RANGE, 0x03 },
+    // disable low-power mode
+    { REGA_PWR_CONF, 0 },
+    { REGA_PWR_CTRL, 0x04 },
+    // setup FIFO for streaming X,Y,Z
+    { REGA_FIFO_CONFIG0, 0x00 },
+    { REGA_FIFO_CONFIG1, 0x50 },
+};
+
+bool AP_InertialSensor_BMI088::setup_accel_config(void)
+{
+    if (done_accel_config) {
+        return true;
+    }
+    accel_config_count++;
+    for (uint8_t i=0; i<ARRAY_SIZE(accel_config); i++) {
+        uint8_t v;
+        if (!read_accel_registers(accel_config[i].reg, &v, 1)) {
+            return false;
+        }
+        if (v == accel_config[i].value) {
+            continue;
+        }
+        if (!write_accel_register(accel_config[i].reg, accel_config[i].value)) {
+            return false;
+        }
+    }
+    done_accel_config = true;
+    hal.console->printf("BMI088: accel config OK (%u tries)\n", accel_config_count);
+    return true;
+}
+
 /*
   probe and initialise accelerometer
  */
@@ -162,30 +200,8 @@ bool AP_InertialSensor_BMI088::accel_init()
         return false;
     }
 
-    // setup normal mode for DLPF, with 1600Hz ODR
-    if (!write_accel_register(REGA_CONF, 0xAC)) {
-        return false;
-    }
-        
-    // setup 24g range
-    if (!write_accel_register(REGA_RANGE, 0x03)) {
-        return false;
-    }
-        
-    // disable low-power mode
-    if (!write_accel_register(REGA_PWR_CONF, 0)) {
-        return false;
-    }
-    if (!write_accel_register(REGA_PWR_CTRL, 0x04)) {
-        return false;
-    }
-        
-    // setup FIFO for streaming X,Y,Z
-    if (!write_accel_register(REGA_FIFO_CONFIG0, 0x00)) {
-        return false;
-    }
-    if (!write_accel_register(REGA_FIFO_CONFIG1, 0x50)) {
-        return false;
+    if (!setup_accel_config()) {
+        hal.console->printf("BMI088: delaying accel config\n");
     }
 
     hal.console->printf("BMI088: found accel\n");
@@ -255,6 +271,9 @@ bool AP_InertialSensor_BMI088::init()
  */
 void AP_InertialSensor_BMI088::read_fifo_accel(void)
 {
+    if (!setup_accel_config()) {
+        return;
+    }
     uint8_t len[2];
     if (!read_accel_registers(REGA_FIFO_LEN0, len, 2)) {
         _inc_accel_error_count(accel_instance);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
index 2ddc319422..3aca89edac 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
@@ -69,6 +69,11 @@ private:
      */
     bool write_accel_register(uint8_t reg, uint8_t v);
 
+    /*
+      configure accel registers
+     */
+    bool setup_accel_config(void);
+
     AP_HAL::OwnPtr<AP_HAL::Device> dev_accel;
     AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
 
@@ -76,4 +81,7 @@ private:
     uint8_t gyro_instance;
     enum Rotation rotation;
     uint8_t temperature_counter;
+
+    bool done_accel_config;
+    uint32_t accel_config_count;
 };