From 2aa7a01a971aad366da7ced72bf800b2bb51d070 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 2 Jul 2024 13:28:53 +0100 Subject: [PATCH 1/5] AP_BatteryMonitor: rename unhealty failsafe to missing, add missing method and arming check --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 4 ++-- libraries/AP_BattMonitor/AP_BattMonitor.h | 2 +- libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp | 13 ++++++++++--- libraries/AP_BattMonitor/AP_BattMonitor_Backend.h | 3 +++ 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index d1bdba325878c..52d7643051d6a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/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; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 0dbcf93bb756e..fa204f7e93d9f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/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 }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 728e3b918adca..e606c1892b091 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/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() { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index a33c316d70d7b..54436407b92c5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/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 From 1c1875bd3201bc427153a9a0b4b5061a42b547ee Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 2 Jul 2024 13:40:23 +0100 Subject: [PATCH 2/5] AP_Battmonitor: ESC: mark as missing if all expected ESCs are not found --- .../AP_BattMonitor/AP_BattMonitor_ESC.cpp | 19 +++++++++++++++++++ libraries/AP_BattMonitor/AP_BattMonitor_ESC.h | 8 ++++++++ 2 files changed, 27 insertions(+) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp index a1c309a6e60ef..20837216767ac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp +++ b/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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h index 4d3eb33bacecc..b0c230384fa1f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h @@ -46,6 +46,11 @@ 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; @@ -53,6 +58,9 @@ class AP_BattMonitor_ESC :public AP_BattMonitor_Backend bool have_current; bool have_temperature; float delta_mah; + + uint8_t esc_count; + bool missing_esc; }; #endif // AP_BATTERY_ESC_ENABLED From 30748093eb11b2785d5e8d5f7c6a2bfa9d150149 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 2 Jul 2024 13:41:18 +0100 Subject: [PATCH 3/5] AP_BattMonitor: Sum: mark as missing if expected monitors are not found --- libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp | 16 ++++++++++++++++ libraries/AP_BattMonitor/AP_BattMonitor_Sum.h | 6 ++++++ 2 files changed, 22 insertions(+) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp index 2a7401d3267a8..e4156004e8de0 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp +++ b/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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h index 1e882bb3339ff..dd342da600f1a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h +++ b/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 From 2ccd89c98ada649bd8c69c126b8da4c49282423f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 2 Jul 2024 13:44:20 +0100 Subject: [PATCH 4/5] AP_HAL_SITL: AnalogIn: return false for set pin to `ANALOG_INPUT_NONE` --- libraries/AP_HAL_SITL/AnalogIn.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/AnalogIn.cpp b/libraries/AP_HAL_SITL/AnalogIn.cpp index 84800fc0f2af9..6f3d17d9a91fe 100644 --- a/libraries/AP_HAL_SITL/AnalogIn.cpp +++ b/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() { From 96de1c11fd7ee7d6cd26ff67121db2e189b35670 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 2 Jul 2024 16:37:16 +0100 Subject: [PATCH 5/5] Tools: autotest: Copter: add battery missing prearm and failsafe test --- Tools/autotest/arducopter.py | 64 ++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a80de7973d7ec..eabf1b0c1e034 100644 --- a/Tools/autotest/arducopter.py +++ b/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,