diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index 33a9432a71..444e1df333 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -7,7 +7,7 @@ #include #include "AP_BattMonitor_Backend.h" -#define AP_BATTMONITOR_SMBUS_MAX_CELLS 4 +#define AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds class AP_BattMonitor_SMBus : public AP_BattMonitor_Backend { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp index 0893505484..fa065db92a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp @@ -43,6 +43,7 @@ void AP_BattMonitor_SMBus_I2C::read() if (read_word(BATTMONITOR_SMBUS_VOLTAGE, data)) { _state.voltage = (float)data / 1000.0f; _state.last_time_micros = tnow; + _state.healthy = true; } // read current @@ -50,6 +51,11 @@ void AP_BattMonitor_SMBus_I2C::read() _state.current_amps = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; _state.last_time_micros = tnow; } + + // timeout after 5 seconds + if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { + _state.healthy = false; + } } // read word from register diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp index b75726de75..6ef5e321b2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp @@ -55,6 +55,13 @@ void AP_BattMonitor_SMBus_PX4::read() _state.voltage = batt_status.voltage_v; _state.current_amps = batt_status.current_a; _state.last_time_micros = hal.scheduler->micros(); + _state.current_total_mah = batt_status.discharged_mah; + _state.healthy = true; + } + } else if (_state.healthy) { + // timeout after 5 seconds + if ((hal.scheduler->micros() - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { + _state.healthy = false; } } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h index e59e0441a6..25dab90da7 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h @@ -34,7 +34,6 @@ public: private: int _batt_sub; // orb subscription description - uint64_t _last_timestamp; // time of last update (used to avoid processing old reports) }; #endif // AP_BATTMONITOR_SMBUS_PX4_H