AP_BattMonitor: Sum, ESC: all selected items must be found to not be missing

Tools/autotest/arducopter.py

@@ -1115,6 +1115,69 @@ def test_battery_failsafe(self, timeout=300):
 
         self.progress("All Battery failsafe tests complete")
 
+    def BatteryMissing(self):
+        ''' Test battery missing pre-arm and failsafe'''
+        self.context_push()
+
+        # Sum monitor on primary with two analog monitors
+        # analog can be made unhealthy by setting a invalid pin
+        self.set_parameters({
+            'BATT_MONITOR': 10,
+            'BATT2_MONITOR': 3,
+            'BATT3_MONITOR': 3,
+        })
+        self.context_push()
+
+        # Should be good to arm with no changes
+        self.reboot_sitl()
+        self.wait_ready_to_arm()
+
+        # Make one monitor unhealthy, this should result in missing prearm
+        self.set_parameters({
+            'BATT2_VOLT_PIN': -1,
+        })
+
+        self.drain_mav()
+
+        # Battery 2 should go unhealthy immediately
+        self.assert_prearm_failure("Battery 2 unhealthy", other_prearm_failures_fatal=False)
+
+        # Sum goes missing after 5 seconds, because its a lower number instance its pre-arm takes priority
+        self.assert_prearm_failure("Battery 1 missing", other_prearm_failures_fatal=False, timeout=20)
+
+        # With both monitors unhealthy the sum should also be unhealthy
+        self.set_parameters({
+            'BATT3_VOLT_PIN': -1,
+        })
+
+        self.drain_mav()
+        self.assert_prearm_failure("Battery 1 unhealthy", other_prearm_failures_fatal=False)
+
+        # Return both monitors to health
+        self.context_pop()
+        self.wait_ready_to_arm()
+
+        # take off and then trigger in flight
+        self.takeoff(10, mode="LOITER")
+        self.set_parameters({
+            'BATT2_VOLT_PIN': -1,
+        })
+
+        # Sum monitor will change to missing if one monitor goes unhealthy
+        self.wait_statustext("Battery 1 is missing")
+
+        # Should also see failsafe individually
+        self.set_parameters({
+            'BATT3_VOLT_PIN': -1,
+        })
+
+        self.wait_statustext("Battery 3 is missing")
+
+        # Done, reset params and reboot to clear failsafe
+        self.land_and_disarm()
+        self.context_pop()
+        self.reboot_sitl()
+
     def VibrationFailsafe(self):
         '''Test Vibration Failsafe'''
         self.context_push()
@@ -10446,6 +10509,7 @@ def tests1c(self):
         '''return list of all tests'''
         ret = ([
              self.BatteryFailsafe,
+             self.BatteryMissing,
              self.VibrationFailsafe,
              self.EK3AccelBias,
              self.StabilityPatch,

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -858,7 +858,7 @@ void AP_BattMonitor::check_failsafes(void)
             switch (type) {
                 case Failsafe::None:
                     continue; // should not have been called in this case
-                case Failsafe::Unhealthy:
+                case Failsafe::Missing:
                     // Report only for unhealthy, could add action param in the future
                     action = 0;
                     type_str = "missing, last:";
@@ -1090,7 +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:
+    case Failsafe::Missing:
         if (get_mavlink_fault_bitmask(instance) != 0 || !healthy()) {
             return MAV_BATTERY_CHARGE_STATE_UNHEALTHY;
         }

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -81,7 +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,
+        Missing,
         Low,
         Critical
     };

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -168,9 +168,8 @@ 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 (missing()) {
+        return AP_BattMonitor::Failsafe::Missing;
     }
 
     // if we've gotten this far then battery is ok
@@ -203,6 +202,7 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
                                 !(_params._low_voltage > _params._critical_voltage);
 
     bool result = update_check(buflen, buffer, !_state.healthy, "unhealthy");
+    result = result && update_check(buflen, buffer, missing(), "missing");
     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");
@@ -259,6 +259,13 @@ void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_c
     }
 }
 
+// Return true if the battery monitor is missing
+bool AP_BattMonitor_Backend::missing() const
+{
+    // Consider as missing if unhealthy for more than 5 seconds
+    return (AP_HAL::millis() - _state.last_healthy_ms) > 5000;
+}
+
 #if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
 void AP_BattMonitor_Backend::update_esc_telem_outbound()
 {

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -117,6 +117,9 @@ class AP_BattMonitor_Backend
     void check_failsafe_types(bool &low_voltage, bool &low_capacity, bool &critical_voltage, bool &critical_capacity) const;
     void update_consumed(AP_BattMonitor::BattMonitor_State &state, uint32_t dt_us);
 
+    // Return true if the monitor is missing
+    virtual bool missing() const;
+
 private:
     // resistance estimate
     uint32_t    _resistance_timer_ms;    // system time of last resistance estimate update

libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp

@@ -99,6 +99,19 @@ void AP_BattMonitor_ESC::read(void)
         }
     }
 
+    // Max number of ESCs ever seen
+    esc_count = MAX(esc_count, voltage_escs);
+
+    // Set missing flag if the expected number of ESCs is not found
+    // The monitor remains healthy because it can still give a partial voltage and current reading
+    if (all_enabled) {
+        // If using all ESCs then the count must never reduce
+        missing_esc = voltage_escs != esc_count;
+    } else {
+        // If using a mask of ESCs all selected must be found
+        missing_esc = voltage_escs != __builtin_popcount(_mask);
+    }
+
     if (voltage_escs > 0) {
         _state.voltage = voltage_sum / voltage_escs;
         _state.healthy = true;
@@ -137,4 +150,10 @@ bool AP_BattMonitor_ESC::reset_remaining(float percentage)
     return false;
 }
 
+// Return true if the monitor is missing an ESC
+bool AP_BattMonitor_ESC::missing() const
+{
+    return missing_esc || AP_BattMonitor_Backend::missing();
+}
+
 #endif // AP_BATTERY_ESC_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_ESC.h

@@ -46,13 +46,21 @@ class AP_BattMonitor_ESC :public AP_BattMonitor_Backend
 
     static const struct AP_Param::GroupInfo var_info[];
 
+protected:
+
+    // Return true if the monitor is missing an ESC
+    bool missing() const override;
+
 private:
 
     AP_Int32  _mask;
 
     bool have_current;
     bool have_temperature;
     float delta_mah;
+
+    uint8_t esc_count;
+    bool missing_esc;
 };
 
 #endif  // AP_BATTERY_ESC_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp

@@ -54,6 +54,7 @@ AP_BattMonitor_Sum::read()
     float current_sum = 0;
     uint8_t current_count = 0;
 
+    bool all_healthy = true;
     for (uint8_t i=0; i<_mon.num_instances(); i++) {
         if (i == _instance) {
             // never include self
@@ -68,6 +69,7 @@ AP_BattMonitor_Sum::read()
             continue;
         }
         if (!_mon.healthy(i)) {
+            all_healthy = false;
             continue;
         }
         voltage_sum += _mon.voltage(i);
@@ -78,6 +80,13 @@ AP_BattMonitor_Sum::read()
             current_count++;
         }
     }
+
+    // The monitor remains healthy because it can still give a partial voltage and current reading
+    // A missing failsafe will be triggered if not all healthy for 5 seconds
+    if (all_healthy) {
+        _last_all_healthy_ms = AP_HAL::millis();
+    }
+
     const uint32_t tnow_us = AP_HAL::micros();
     const uint32_t dt_us = tnow_us - _state.last_time_micros;
 
@@ -98,4 +107,11 @@ AP_BattMonitor_Sum::read()
     }
 }
 
+// Return true if the monitor is missing a instance
+bool AP_BattMonitor_Sum::missing() const
+{
+    const bool all_healthy = (AP_HAL::millis() - _last_all_healthy_ms) < 5000;
+    return !all_healthy || AP_BattMonitor_Backend::missing();
+}
+
 #endif  // AP_BATTERY_SUM_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Sum.h

@@ -26,11 +26,17 @@ class AP_BattMonitor_Sum : public AP_BattMonitor_Backend
 
     static const struct AP_Param::GroupInfo var_info[];
 
+protected:
+
+    // Return true if the monitor is missing a instance
+    bool missing() const override;
+
 private:
 
     AP_Int16  _sum_mask;
     uint8_t _instance;
     bool _has_current;
+    uint32_t _last_all_healthy_ms;
 };
 
 #endif  // AP_BATTERY_SUM_ENABLED

libraries/AP_HAL_SITL/AnalogIn.cpp

@@ -62,7 +62,7 @@ float ADCSource::read_latest() {
 
 bool ADCSource::set_pin(uint8_t pin) {
     _pin = pin;
-    return true;
+    return pin != ANALOG_INPUT_NONE;
 }
 
 void AnalogIn::init() {