From c5652b2084639abc341b31cfbfc52ba77f488650 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Thu, 12 Feb 2026 17:18:55 +0100 Subject: [PATCH] escChecks: param reorg Reorganise parameters fix esc & motor indices set failsafe flags --- .../HealthAndArmingChecks_params.c | 115 ++++++++++++++++++ .../HealthAndArmingChecks/checks/escCheck.cpp | 79 ++++++------ .../HealthAndArmingChecks/checks/escCheck.hpp | 21 ++-- .../failure_detector/FailureDetector.cpp | 1 - .../failure_detector/FailureDetector.hpp | 12 +- .../failure_detector_params.c | 89 +------------- 6 files changed, 174 insertions(+), 143 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks_params.c diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks_params.c b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks_params.c new file mode 100644 index 0000000000..beafbf0212 --- /dev/null +++ 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 + */ + +#include +#include + +/** + * 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); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 03af85fc83..6e8a6f5344 100644 --- 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 * @@ -150,8 +150,8 @@ uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, con * */ reporter.healthFailure(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(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( 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; } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp index 20e1cf15c2..d8f457a842 100644 --- 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) _param_com_arm_chk_escs, + (ParamBool) _param_fd_act_en, - (ParamFloat) _param_fd_act_mot_thr, - (ParamFloat) _param_fd_act_mot_c2t, - (ParamInt) _param_fd_act_mot_tout, - (ParamFloat) _param_fd_act_low_off, - (ParamFloat) _param_fd_act_high_off); + (ParamFloat) _param_motfail_thr, + (ParamFloat) _param_motfail_c2t, + (ParamInt) _param_motfail_tout, + (ParamFloat) _param_motfail_low_off, + (ParamFloat) _param_motfail_high_off); + }; diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index da82194243..5270367929 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -217,4 +217,3 @@ void FailureDetector::updateImbalancedPropStatus() } } } - diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index d29691d0bf..6cc9816a6d 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -131,15 +131,5 @@ private: (ParamFloat) _param_fd_fail_p_ttri, (ParamBool) _param_fd_ext_ats_en, (ParamInt) _param_fd_ext_ats_trig, - (ParamInt) _param_escs_en, - (ParamInt) _param_fd_imb_prop_thr, - - // Actuator failure - (ParamBool) _param_fd_act_en, - (ParamFloat) _param_fd_act_mot_thr, - (ParamFloat) _param_fd_act_mot_c2t, - (ParamInt) _param_fd_act_mot_tout, - (ParamFloat) _param_fd_act_low_off, - (ParamFloat) _param_fd_act_high_off - ) + (ParamInt) _param_fd_imb_prop_thr) }; diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c index 9324820472..5c99d49760 100644 --- 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);