commander: move battery handling into arming checks

2026-10-23 03:33:34 +08:00 · 2022-09-01 17:07:23 -04:00
parent 5cb44a521c
commit cfe3d793bf
6 changed files with 231 additions and 178 deletions
@@ -33,14 +33,202 @@

 #include "batteryCheck.hpp"

+#include <px4_platform_common/events.h>
+
 using namespace time_literals;

+using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t;
+static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast<uint8_t>(battery_fault_reason_t::_max) + 1)
+	      , "Battery fault flags mismatch!");
+
+static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason)
+{
+	switch (battery_fault_reason) {
+	case battery_fault_reason_t::deep_discharge: return "under voltage";
+
+	case battery_fault_reason_t::voltage_spikes: return "over voltage";
+
+	case battery_fault_reason_t::cell_fail: return "cell fault";
+
+	case battery_fault_reason_t::over_current: return "over current";
+
+	case battery_fault_reason_t::fault_temperature: return "critical temperature";
+
+	case battery_fault_reason_t::under_temperature: return "under temperature";
+
+	case battery_fault_reason_t::incompatible_voltage: return "voltage mismatch";
+
+	case battery_fault_reason_t::incompatible_firmware: return "incompatible firmware";
+
+	case battery_fault_reason_t::incompatible_model: return "incompatible model";
+
+	case battery_fault_reason_t::hardware_fault: return "hardware fault";
+
+	case battery_fault_reason_t::over_temperature: return "near temperature limit";
+
+	}
+
+	return "";
+};
+
+
+using battery_mode_t = events::px4::enums::battery_mode_t;
+static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast<uint8_t>(battery_mode_t::_max) + 1)
+	      , "Battery mode flags mismatch!");
+static constexpr const char *battery_mode_str(battery_mode_t battery_mode)
+{
+	switch (battery_mode) {
+	case battery_mode_t::autodischarging: return "auto discharging";
+
+	case battery_mode_t::hotswap: return "hot-swap";
+
+	default: return "unknown";
+	}
+}
+
+
 void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (!context.status().battery_healthy) {
+	int battery_required_count = 0;
+	bool battery_has_fault = false;
+	// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
+	// option is to check if ANY of them have a warning, and specifically find which one has the most
+	// urgent warning.
+	uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
+	// To make sure that all connected batteries are being regularly reported, we check which one has the
+	// oldest timestamp.
+	hrt_abstime oldest_update = hrt_absolute_time();
+	float worst_battery_time_s{NAN};
+	int num_connected_batteries{0};

+	for (auto &battery_sub : _battery_status_subs) {
+		int index = battery_sub.get_instance();
+		battery_status_s battery;
+
+		if (!battery_sub.copy(&battery)) {
+			continue;
+		}
+
+		if (battery.is_required) {
+			battery_required_count++;
+		}
+
+		if (context.isArmed()) {
+
+			if (!battery.connected) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::battery, events::ID("check_battery_disconnected"),
+								events::Log::Emergency, "Battery {1} disconnected", index + 1);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %i disconnected\t", index + 1);
+				}
+
+				// trigger a battery failsafe action if a battery disconnects in flight
+				worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+			}
+
+			if (battery.mode != 0) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t, events::px4::enums::battery_mode_t>(NavModes::All, health_component_t::battery,
+						events::ID("check_battery_mode"),
+						events::Log::Critical, "Battery {1} mode: {2}", index + 1, static_cast<battery_mode_t>(battery.mode));
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %d is in %s mode!\t", index + 1,
+							     battery_mode_str(static_cast<battery_mode_t>(battery.mode)));
+				}
+			}
+		}
+
+		if (battery.connected) {
+			++num_connected_batteries;
+
+			if (battery.warning > worst_warning) {
+				worst_warning = battery.warning;
+			}
+
+			if (battery.timestamp < oldest_update) {
+				oldest_update = battery.timestamp;
+			}
+
+			if (battery.faults > 0) {
+				battery_has_fault = true;
+
+				for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(battery_fault_reason_t::_max); fault_index++) {
+					if (battery.faults & (1 << fault_index)) {
+						events::px4::enums::suggested_action_t action = context.isArmed() ?
+								events::px4::enums::suggested_action_t::land :
+								events::px4::enums::suggested_action_t::none;
+
+						/* EVENT
+						 * @description
+						 * The battery reported a failure which might be dangerous to fly with.
+						 * Manufacturer error code: {4}
+						 */
+						reporter.healthFailure<uint8_t, battery_fault_reason_t, events::px4::enums::suggested_action_t, uint32_t>
+						(NavModes::All, health_component_t::battery, events::ID("check_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning},
+						 "Battery {1}: {2}. {3}", index + 1, static_cast<battery_fault_reason_t>(fault_index), action, battery.custom_faults);
+
+						if (reporter.mavlink_log_pub()) {
+							mavlink_log_emergency(reporter.mavlink_log_pub(), "Battery %d: %s. %s \t", index + 1,
+									      battery_fault_reason_str(
+										      static_cast<battery_fault_reason_t>(fault_index)),
+									      context.isArmed() ? "Land now!" : "");
+						}
+					}
+				}
+			}
+
+			if (PX4_ISFINITE(battery.time_remaining_s)
+			    && (!PX4_ISFINITE(worst_battery_time_s)
+				|| (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) {
+				worst_battery_time_s = battery.time_remaining_s;
+			}
+		}
+	}
+
+	if (context.isArmed()) {
+		if (worst_warning > reporter.failsafeFlags().battery_warning) {
+			reporter.failsafeFlags().battery_warning = worst_warning;
+		}
+
+	} else {
+		reporter.failsafeFlags().battery_warning = worst_warning;
+	}
+
+	if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE) {
+		bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL;
+		NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None;
+		events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning;
 		/* EVENT
 		 */
+		reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level,
+					    "Low battery");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_emergency(reporter.mavlink_log_pub(), "Preflight Fail: low battery\t");
+		}
+
+	}
+
+	rtlEstimateCheck(context, reporter, worst_battery_time_s);
+
+	reporter.failsafeFlags().battery_unhealthy =
+		// All connected batteries are regularly being published
+		hrt_elapsed_time(&oldest_update) > 5_s
+		// There is at least one connected battery (in any slot)
+		|| num_connected_batteries < battery_required_count
+		// No currently-connected batteries have any fault
+		|| battery_has_fault;
+
+	if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already
+		/* EVENT
+		 * @description
+		 * Make sure all batteries are connected.
+		 */
 		reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
 				       events::Log::Error, "Battery unhealthy");

@@ -49,6 +237,29 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}

-	reporter.setIsPresent(health_component_t::battery); // assume it's present
+	if (num_connected_batteries > 0) {
+		reporter.setIsPresent(health_component_t::battery);
+	}
+
+}
+
+void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, float worst_battery_time_s)
+{
+	rtl_time_estimate_s rtl_time_estimate;
+
+	// Compare estimate of RTL time to estimate of remaining flight time
+	reporter.failsafeFlags().battery_low_remaining_time = _rtl_time_estimate_sub.copy(&rtl_time_estimate)
+			&& (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s
+			&& rtl_time_estimate.valid
+			&& context.isArmed()
+			&& PX4_ISFINITE(worst_battery_time_s)
+			&& rtl_time_estimate.safe_time_estimate >= worst_battery_time_s;
+
+	if (reporter.failsafeFlags().battery_low_remaining_time) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_rem_flight_time_low"),
+					    events::Log::Error, "Remaining flight time low");
+	}

 }
@@ -36,6 +36,9 @@
 #include "../Common.hpp"

 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/rtl_time_estimate.h>

 class BatteryChecks : public HealthAndArmingCheckBase
 {
@@ -46,4 +49,9 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;

 private:
+	void rtlEstimateCheck(const Context &context, Report &reporter, float worst_battery_time_s);
+
+	uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
+	uORB::Subscription					_rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
+
 };