From 9eab488dd8e41cbd8e7630afeca170cca2e37acb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 21 Jun 2022 11:58:58 +1000
Subject: [PATCH] AP_BattMonitor: allow INA2xx battery monitors to be powered
 after boot

the device doesn't show up till main battery is connected, so we need
this change to allow for separate avionics battery and main battery
with avionics battery powered on first
---
 .../AP_BattMonitor/AP_BattMonitor_Backend.cpp |  4 +-
 .../AP_BattMonitor/AP_BattMonitor_INA2xx.cpp  | 39 ++++++++++++++-----
 .../AP_BattMonitor/AP_BattMonitor_INA2xx.h    |  9 ++++-
 3 files changed, 39 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
index 758f2464d2..6863f6ca26 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
@@ -176,7 +176,8 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
                                 is_positive(_params._low_voltage) &&
                                 (_params._low_voltage < _params._critical_voltage);
 
-    bool result =      update_check(buflen, buffer, below_arming_voltage, "below minimum arming voltage");
+    bool result = update_check(buflen, buffer, !_state.healthy, "unhealthy");
+    result = result && update_check(buflen, buffer, below_arming_voltage, "below minimum arming voltage");
     result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity");
     result = result && update_check(buflen, buffer, low_voltage,  "low voltage failsafe");
     result = result && update_check(buflen, buffer, low_capacity, "low capacity failsafe");
@@ -184,7 +185,6 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
     result = result && update_check(buflen, buffer, critical_capacity, "critical capacity failsafe");
     result = result && update_check(buflen, buffer, fs_capacity_inversion, "capacity failsafe critical > low");
     result = result && update_check(buflen, buffer, fs_voltage_inversion, "voltage failsafe critical > low");
-    result = result && update_check(buflen, buffer, !_state.healthy, "unhealthy");
 
     return result;
 }
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
index 6ecc8fde94..55654e73eb 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
@@ -61,14 +61,19 @@ void AP_BattMonitor_INA2XX::init(void)
     if (!dev) {
         return;
     }
+    // register now and configure in the timer callbacks
+    dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA2XX::timer, void));
+}
+
+void AP_BattMonitor_INA2XX::configure(void)
+{
+    WITH_SEMAPHORE(dev->get_semaphore());
 
     int16_t config = 0;
-    WITH_SEMAPHORE(dev->get_semaphore());
     if (!write_word(REG_CONFIG, REG_CONFIG_RESET) ||
         !write_word(REG_CONFIG, REG_CONFIG_DEFAULT) ||
         !read_word(REG_CONFIG, config) ||
         config != REG_CONFIG_DEFAULT) {
-        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA2XX: Failed to find device 0x%04x", unsigned(config));
         return;
     }
 
@@ -81,16 +86,10 @@ void AP_BattMonitor_INA2XX::init(void)
     if (!write_word(REG_CONFIG, REG_CONFIG_RESET) || // reset
         !write_word(REG_CONFIG, conf) ||
         !write_word(REG_CALIBRATION, cal)) {
-        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA2XX: Failed to configure device");
         return;
     }
 
-
-    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA2XX: found monitor on I2C:%u:%02x", unsigned(i2c_bus), unsigned(i2c_address));
-
-    if (dev) {
-        dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA2XX::timer, void));
-    }
+    configured = true;
 }
 
 /// read the battery_voltage and current, should be called at 10hz
@@ -146,11 +145,33 @@ bool AP_BattMonitor_INA2XX::write_word(const uint8_t reg, const uint16_t data) c
 
 void AP_BattMonitor_INA2XX::timer(void)
 {
+    // allow for power-on after boot
+    if (!configured) {
+        uint32_t now = AP_HAL::millis();
+        if (now - last_configure_ms > 200) {
+            // try contacting the device at 5Hz
+            last_configure_ms = now;
+            configure();
+        }
+        if (!configured) {
+            // waiting for the device to respond
+            return;
+        }
+    }
+
     int16_t bus_voltage, current;
+
     if (!read_word(REG_BUS_VOLTAGE, bus_voltage) ||
         !read_word(REG_CURRENT, current)) {
+        failed_reads++;
+        if (failed_reads > 10) {
+            // device has disconnected, we need to reconfigure it
+            configured = false;
+        }
         return;
     }
+    failed_reads = 0;
+
     WITH_SEMAPHORE(accumulate.sem);
     accumulate.volt_sum += bus_voltage * voltage_LSB;
     accumulate.current_sum += current * current_LSB;
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h
index 8c4b77a227..096cc715c6 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h
@@ -26,20 +26,25 @@ public:
     bool reset_remaining(float percentage) override { return false; }
     bool get_cycle_count(uint16_t &cycles) const override { return false; }
 
-    virtual void init(void) override;
-    virtual void read() override;
+    void init(void) override;
+    void read() override;
 
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
 
+    void configure(void);
     bool read_word(const uint8_t reg, int16_t& data) const;
     bool write_word(const uint8_t reg, const uint16_t data) const;
     void timer(void);
 
     AP_Int8 i2c_bus;
     AP_Int8 i2c_address;
+    bool configured;
+    bool callback_registered;
+    uint32_t failed_reads;
+    uint32_t last_configure_ms;
 
     struct {
         uint16_t count;