diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 8c766b4e362f9..d1bdba325878c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -701,6 +701,7 @@ void AP_BattMonitor::read() } #endif + const uint32_t now_ms = AP_HAL::millis(); for (uint8_t i=0; i<_num_instances; i++) { if (drivers[i] == nullptr) { continue; @@ -719,6 +720,11 @@ void AP_BattMonitor::read() drivers[i]->update_esc_telem_outbound(); #endif + // Update last heathy timestamp + if (state[i].healthy) { + state[i].last_healthy_ms = now_ms; + } + #if HAL_LOGGING_ENABLED if (logger != nullptr && logger->should_log(_log_battery_bit)) { const uint64_t time_us = AP_HAL::micros64(); @@ -852,6 +858,11 @@ void AP_BattMonitor::check_failsafes(void) switch (type) { case Failsafe::None: continue; // should not have been called in this case + case Failsafe::Unhealthy: + // Report only for unhealthy, could add action param in the future + action = 0; + type_str = "missing, last:"; + break; case Failsafe::Low: action = _params[i]._failsafe_low_action; type_str = "low"; @@ -1079,6 +1090,7 @@ MAV_BATTERY_CHARGE_STATE AP_BattMonitor::get_mavlink_charge_state(const uint8_t switch (state[instance].failsafe) { case Failsafe::None: + case Failsafe::Unhealthy: if (get_mavlink_fault_bitmask(instance) != 0 || !healthy()) { return MAV_BATTERY_CHARGE_STATE_UNHEALTHY; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index f03e9976320be..0dbcf93bb756e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -81,6 +81,7 @@ class AP_BattMonitor // battery failsafes must be defined in levels of severity so that vehicles wont fall backwards enum class Failsafe : uint8_t { None = 0, + Unhealthy, Low, Critical }; @@ -153,6 +154,7 @@ class AP_BattMonitor float resistance; // resistance, in Ohms, calculated by comparing resting voltage vs in flight voltage Failsafe failsafe; // stage failsafe the battery is in bool healthy; // battery monitor is communicating correctly + uint32_t last_healthy_ms; // Time when monitor was last healthy bool is_powering_off; // true when power button commands power off bool powerOffNotified; // only send powering off notification once uint32_t time_remaining; // remaining battery time diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 195dbded04047..728e3b918adca 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -168,6 +168,11 @@ AP_BattMonitor::Failsafe AP_BattMonitor_Backend::update_failsafes(void) return AP_BattMonitor::Failsafe::Low; } + // 5 second health timeout + if ((now - _state.last_healthy_ms) > 5000) { + return AP_BattMonitor::Failsafe::Unhealthy; + } + // if we've gotten this far then battery is ok return AP_BattMonitor::Failsafe::None; }