AP_BatteryMonitor: add missing failsafe with no action

ArduPilot · Jun 22, 2024 · 78939f7 · 78939f7
1 parent 96682b1
commit 78939f7
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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;
 }