diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 7463561746..50b8313d42 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 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;