escChecks: param reorg

Reorganise parameters fix esc & motor indices set failsafe flags
2026-03-24 02:24:09 +08:00 · 2026-02-12 17:18:55 +01:00
parent 03fc051c29
commit c5652b2084
6 changed files with 174 additions and 143 deletions
--- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks_params.c
+++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks_params.c
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file health_and_arming_checks_params.c
+ *
+ * Parameters used by the Health and Arming Checks module.
+ *
+ * @author Nick Truttmann <nick.truttmann@icloud.com>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <parameters/param.h>
+
+/**
+ * Motor Failure Thrust Threshold
+ *
+ * Failure detection per motor only triggers above this thrust value.
+ * Set to 1 to disable the detection.
+ *
+ * @group Motor Failure
+ * @unit norm
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ */
+PARAM_DEFINE_FLOAT(MOTFAIL_THR, 0.2f);
+
+/**
+ * Motor Failure Current/Throttle Scale
+ *
+ * Determines the slope between expected steady state current and linearized, normalized thrust command.
+ * E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
+ * FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
+ *
+ * @group Motor Failure
+ * @min 0.0
+ * @max 50.0
+ * @unit A/%
+ * @decimal 2
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(MOTFAIL_C2T, 35.f);
+
+/**
+ * Motor Failure Hysteresis Time
+ *
+ * Motor failure only triggers after current thresholds are exceeded for this time.
+ *
+ * @group Motor Failure
+ * @unit ms
+ * @min 10
+ * @max 10000
+ * @increment 100
+ */
+PARAM_DEFINE_INT32(MOTFAIL_TOUT, 1000);
+
+/**
+ * Undercurrent motor failure limit offset
+ *
+ * threshold = FD_ACT_MOT_C2T * thrust - FD_ACT_LOW_OFF
+ *
+ * @group Motor Failure
+ * @min 0
+ * @max 30
+ * @unit A
+ * @decimal 2
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(MOTFAIL_LOW_OFF, 10.f);
+
+/**
+ * Overcurrent motor failure limit offset
+ *
+ * threshold = FD_ACT_MOT_C2T * thrust + FD_ACT_HIGH_OFF
+ *
+ * @group Motor Failure
+ * @min 0
+ * @max 30
+ * @unit A
+ * @decimal 2
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(MOTFAIL_HIGH_OFF, 10.f);
--- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
@@ -83,6 +83,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
 		if (esc_status.esc_count <= 0) {
 			return;
 		}
+
 		uint16_t mask = 0;

 		mask |= checkEscOnline(context, reporter, esc_status);
@@ -92,8 +93,10 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
 		if (!_param_fd_act_en.get()) {
 			mask |= checkMotorStatus(context, reporter, esc_status);
 		}
+
 		_motor_failure_mask = mask;
 		reporter.setIsPresent(health_component_t::motors_escs);
+		reporter.failsafeFlags().fd_motor_failure = mask != 0;

 	} else if (_param_com_arm_chk_escs.get()
 		   && now - _start_time > esc_telemetry_timeout) { // Wait a bit after startup to allow esc's to init
@@ -116,33 +119,30 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
 uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status)
 {
 	// Check if one or more the ESCs are offline
-	if (!_param_com_arm_chk_escs.get()){
+	if (!_param_com_arm_chk_escs.get()) {
 		return 0;
 	}
+
 	uint16_t mask = 0;
 	char esc_fail_msg[50];
 	esc_fail_msg[0] = '\0';

-	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
+	for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) {

 		// check if mapped
-		if (!math::isInRange(esc_status.esc[index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
-							    uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
+		if (!math::isInRange(esc_status.esc[esc_index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
+				     uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
 			continue; // Skip unmapped ESC status entries
 		}
-		const uint8_t actuator_function_index =
-		esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
-
-		const bool timeout = hrt_absolute_time() > esc_status.esc[index].timestamp + 300_ms;
-		const bool is_offline = (esc_status.esc_online_flags & (1 << index)) == 0;
-

+		const bool timeout = hrt_absolute_time() > esc_status.esc[esc_index].timestamp + 300_ms;
+		const bool is_offline = (esc_status.esc_online_flags & (1 << esc_index)) == 0;

 		// Set failure bits for this motor
 		if (timeout || is_offline) {
-			mask |= (1u << actuator_function_index);
+			mask |= (1u << esc_index);

-			uint8_t motor_index = actuator_function_index + 1;
+			uint8_t esc_nr = esc_index + 1;
 			/* EVENT
 				* @description
 				* <profile name="dev">
@@ -150,8 +150,8 @@ uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, con
 				* </profile>
 				*/
 			reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs, events::ID("check_escs_offline"),
-							events::Log::Critical, "ESC {1} offline", motor_index);
-			snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index);
+							events::Log::Critical, "ESC {1} offline", esc_nr);
+			snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", esc_nr);
 			esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
 		}
 	}
@@ -160,39 +160,39 @@ uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, con
 		mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");

 	}
+
 	return mask;

 }

 uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
 {
-	if (!_param_com_arm_chk_escs.get()){
+	if (!_param_com_arm_chk_escs.get()) {
 		return 0;
 	}

 	uint16_t mask = 0;

-	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
+	for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) {

-		if (!math::isInRange(esc_status.esc[index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
-							    uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
+		if (!math::isInRange(esc_status.esc[esc_index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
+				     uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
 			continue; // Skip unmapped ESC status entries
 		}

 		const uint8_t act_function_index =
-			esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
+			esc_status.esc[esc_index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;

-		if (esc_status.esc[index].failures == 0) {
+		if (esc_status.esc[esc_index].failures == 0) {
 			continue;
-		}
-		else
-		{
+
+		} else {
 			mask |= (1u << act_function_index); // Set bit in mask
 		}


 		for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max); fault_index++) {
-			if (!(esc_status.esc[index].failures & (1 << fault_index))) {
+			if (!(esc_status.esc[esc_index].failures & (1 << fault_index))) {
 				continue;
 			}

@@ -224,22 +224,24 @@ uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, con
 				*/
 			reporter.healthFailure<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>(
 				NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
-				events::Log::Critical, "ESC {1}: {2}", act_function_index + 1, fault_reason_index, action);
+				events::Log::Critical, "ESC {1}: {2}", esc_index + 1, fault_reason_index, action);

 			if (reporter.mavlink_log_pub()) {
-				mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", act_function_index + 1,
+				mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", esc_index + 1,
 						      esc_fault_reason_str(fault_reason_index), user_action);
 			}
 		}
 	}
+
 	return mask;
 }

 uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
 {
-	if (!_param_fd_act_en.get()){
+	if (!_param_fd_act_en.get()) {
 		return 0;
 	}
+
 	const hrt_abstime now = hrt_absolute_time();
 	uint16_t mask = 0;

@@ -254,8 +256,9 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
 			const uint8_t actuator_function_index =
 				esc_status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;

-			if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
-				continue; // Invalid mapping
+			// check if mapped
+			if (!math::isInRange(actuator_function_index, uint8_t(0), uint8_t(actuator_motors_s::NUM_CONTROLS - 1))) {
+				continue; // Skip unmapped ESC status entries
 			}

 			const float current = esc_status.esc[i].esc_current;
@@ -277,12 +280,12 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
 				thrust = fabsf(actuator_motors.control[actuator_function_index]);
 			}

-			bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get();
-			bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get();
-			bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get();
+			bool thrust_above_threshold = thrust > _param_motfail_thr.get();
+			bool current_too_low = current < (thrust * _param_motfail_c2t.get()) - _param_motfail_low_off.get();
+			bool current_too_high = current > (thrust * _param_motfail_c2t.get()) + _param_motfail_high_off.get();

-			_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
-			_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
+			_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_motfail_tout.get() * 1_ms);
+			_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_motfail_tout.get() * 1_ms);

 			if (!_esc_undercurrent_hysteresis[i].get_state()) {
 				// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
@@ -339,15 +342,17 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
 		}

 	}
+
 	return mask;
 }


 void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
 {
-	if (!_param_com_arm_chk_escs.get()){
+	if (!_param_com_arm_chk_escs.get()) {
 		return;
 	}
+
 	hrt_abstime now = hrt_absolute_time();

 	if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
@@ -373,10 +378,14 @@ void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const
 			if (reporter.mavlink_log_pub()) {
 				mavlink_log_critical(reporter.mavlink_log_pub(), "ESC failure: Not all ESCs are armed. Land now!");
 			}
+
+			reporter.failsafeFlags().fd_esc_arming_failure = true;
+
 		}

 	} else {
 		// reset ESC bitfield
 		_esc_arm_hysteresis.set_state_and_update(false, now);
+		reporter.failsafeFlags().fd_esc_arming_failure = false;
 	}
 }
--- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
@@ -49,8 +49,10 @@ public:

 	void checkAndReport(const Context &context, Report &reporter) override;

-	uint16_t getMotorFailureMask() const {
-				return _motor_failure_mask; }
+	uint16_t getMotorFailureMask() const
+	{
+		return _motor_failure_mask;
+	}
 	bool getEscArmStatus() const { return _esc_arm_hysteresis.get_state(); }

 private:
@@ -67,20 +69,19 @@ private:
 	const hrt_abstime _start_time{hrt_absolute_time()};

 	uint16_t _motor_failure_mask{0};
-	esc_status_s _last_esc_status{};
-	bool _esc_status_received{false};
 	bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {};
 	systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
 	systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
 	systemlib::Hysteresis _esc_arm_hysteresis;

-
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs,
+
 					(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
-					(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_act_mot_thr,
-					(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_act_mot_c2t,
-					(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_act_mot_tout,
-					(ParamFloat<px4::params::FD_ACT_LOW_OFF>) _param_fd_act_low_off,
-					(ParamFloat<px4::params::FD_ACT_HIGH_OFF>) _param_fd_act_high_off);
+					(ParamFloat<px4::params::MOTFAIL_THR>) _param_motfail_thr,
+					(ParamFloat<px4::params::MOTFAIL_C2T>) _param_motfail_c2t,
+					(ParamInt<px4::params::MOTFAIL_TOUT>) _param_motfail_tout,
+					(ParamFloat<px4::params::MOTFAIL_LOW_OFF>) _param_motfail_low_off,
+					(ParamFloat<px4::params::MOTFAIL_HIGH_OFF>) _param_motfail_high_off);
+
 };
--- a/src/modules/commander/failure_detector/FailureDetector.cpp
+++ b/src/modules/commander/failure_detector/FailureDetector.cpp
@@ -217,4 +217,3 @@ void FailureDetector::updateImbalancedPropStatus()
 		}
 	}
 }
-
--- a/src/modules/commander/failure_detector/FailureDetector.hpp
+++ b/src/modules/commander/failure_detector/FailureDetector.hpp
@@ -131,15 +131,5 @@ private:
 		(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
-		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
-		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
-
-		// Actuator failure
-		(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
-		(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_act_mot_thr,
-		(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_act_mot_c2t,
-		(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_act_mot_tout,
-		(ParamFloat<px4::params::FD_ACT_LOW_OFF>) _param_fd_act_low_off,
-		(ParamFloat<px4::params::FD_ACT_HIGH_OFF>) _param_fd_act_high_off
-	)
+		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr)
 };
--- a/src/modules/commander/failure_detector/failure_detector_params.c
+++ b/src/modules/commander/failure_detector/failure_detector_params.c
@@ -130,18 +130,6 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0);
 */
 PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);

-/**
- * Enable checks on ESCs that report their arming state.
- *
- * If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state.
- * Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle.
- *
- * @boolean
- *
- * @group Failure Detector
- */
-PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
-
 /**
 * Imbalanced propeller check threshold
 *
@@ -160,85 +148,14 @@ PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
 /**
 * Enable Actuator Failure check
 *
- * If enabled, failure detector will verify that for motors, a minimum amount of ESC current per throttle
+ * If enabled, the HealthAndArmingChecks will verify that for motors, a minimum amount of ESC current per throttle
 * level is being consumed.
 * Otherwise this indicates an motor failure.
+ * This check only works for ESCs that report current consumption.
 *
 * @boolean
 * @reboot_required true
 *
- * @group Failure Detector
+ * @group Motor Failure
 */
 PARAM_DEFINE_INT32(FD_ACT_EN, 0);
-
-/**
- * Motor Failure Thrust Threshold
- *
- * Failure detection per motor only triggers above this thrust value.
- * Set to 1 to disable the detection.
- *
- * @group Failure Detector
- * @unit norm
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.01
- */
-PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
-
-/**
- * Motor Failure Current/Throttle Scale
- *
- * Determines the slope between expected steady state current and linearized, normalized thrust command.
- * E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
- * FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
- *
- * @group Failure Detector
- * @min 0.0
- * @max 50.0
- * @unit A/%
- * @decimal 2
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 35.f);
-
-/**
- * Motor Failure Hysteresis Time
- *
- * Motor failure only triggers after current thresholds are exceeded for this time.
- *
- * @group Failure Detector
- * @unit ms
- * @min 10
- * @max 10000
- * @increment 100
- */
-PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 1000);
-
-/**
- * Undercurrent motor failure limit offset
- *
- * threshold = FD_ACT_MOT_C2T * thrust - FD_ACT_LOW_OFF
- *
- * @group Failure Detector
- * @min 0
- * @max 30
- * @unit A
- * @decimal 2
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(FD_ACT_LOW_OFF, 10.f);
-
-/**
- * Overcurrent motor failure limit offset
- *
- * threshold = FD_ACT_MOT_C2T * thrust + FD_ACT_HIGH_OFF
- *
- * @group Failure Detector
- * @min 0
- * @max 30
- * @unit A
- * @decimal 2
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(FD_ACT_HIGH_OFF, 10.f);