From a98093f87307840ba472516bf2a3eb2139e79516 Mon Sep 17 00:00:00 2001 From: gguidone Date: Tue, 31 Mar 2026 17:45:11 +0200 Subject: [PATCH] feat(commander): add COM_PARA_ACT to configure parachute unhealthy failsafe Previously the parachute unhealthy failsafe was hardcoded to RTL. --- .../checks/parachuteCheck.cpp | 26 ++++++++++++++----- .../checks/parachuteCheck.hpp | 3 ++- src/modules/commander/commander_params.yaml | 13 ++++++++++ src/modules/commander/failsafe/failsafe.cpp | 26 ++++++++++++++++++- src/modules/commander/failsafe/failsafe.h | 10 ++++++- 5 files changed, 69 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp index a3abb2f3fa..9aa03277f7 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp @@ -46,6 +46,10 @@ void ParachuteChecks::checkAndReport(const Context &context, Report &reporter) !context.status().parachute_system_present || !context.status().parachute_system_healthy; + const bool warn_only = (_param_com_para_act.get() == 0); + const events::Log log_level = warn_only ? events::Log::Warning : events::Log::Error; + const NavModes affected_modes = warn_only ? NavModes::None : NavModes::All; + if (!context.status().parachute_system_present) { /* EVENT * @description @@ -55,11 +59,16 @@ void ParachuteChecks::checkAndReport(const Context &context, Report &reporter) * Enabled by COM_PARACHUTE * */ - reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_missing"), - events::Log::Error, "Parachute system missing"); + reporter.healthFailure(affected_modes, health_component_t::parachute, events::ID("check_parachute_missing"), + log_level, "Parachute system missing"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system missing"); + if (warn_only) { + mavlink_log_warning(reporter.mavlink_log_pub(), "Parachute system missing"); + + } else { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system missing"); + } } } else if (!context.status().parachute_system_healthy) { @@ -72,11 +81,16 @@ void ParachuteChecks::checkAndReport(const Context &context, Report &reporter) * Enabled by COM_PARACHUTE * */ - reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_unhealthy"), - events::Log::Error, "Parachute system not ready"); + reporter.healthFailure(affected_modes, health_component_t::parachute, events::ID("check_parachute_unhealthy"), + log_level, "Parachute system not ready"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system not ready"); + if (warn_only) { + mavlink_log_warning(reporter.mavlink_log_pub(), "Parachute system not ready"); + + } else { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system not ready"); + } } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp index 162c03277c..495721c35c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp @@ -45,6 +45,7 @@ public: private: DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamBool) _param_com_parachute + (ParamBool) _param_com_parachute, + (ParamInt) _param_com_para_act ) }; diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml index f265455db8..ac0cc2c0fd 100644 --- a/src/modules/commander/commander_params.yaml +++ b/src/modules/commander/commander_params.yaml @@ -513,6 +513,19 @@ parameters: short: Require MAVLink parachute system to be present and healthy type: boolean default: 0 + COM_PARA_ACT: + description: + short: Parachute system unhealthy failsafe action + long: |- + Action taken when the parachute system is enabled (COM_PARACHUTE=1) but reports + missing or unhealthy status during flight. + type: enum + values: + 0: Warning + 1: Land + 2: Return + default: 2 + increment: 1 COM_ARM_CHK_ESCS: description: short: Enable checks on ESCs that report telemetry diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 25d9728cc6..b4d5469d80 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -412,6 +412,30 @@ FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value) return options; } +FailsafeBase::ActionOptions Failsafe::fromParachuteActParam(int param_value) +{ + ActionOptions options{}; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + + switch (parachute_unhealthy_failsafe_mode(param_value)) { + case parachute_unhealthy_failsafe_mode::Warning: + default: + options.action = Action::Warn; + options.clear_condition = ClearCondition::WhenConditionClears; + break; + + case parachute_unhealthy_failsafe_mode::Land: + options.action = Action::Land; + break; + + case parachute_unhealthy_failsafe_mode::Return: + options.action = Action::RTL; + break; + } + + return options; +} + FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value) { ActionOptions options{}; @@ -575,7 +599,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } // Parachute system health failsafe - CHECK_FAILSAFE(status_flags, parachute_unhealthy, Action::RTL); + CHECK_FAILSAFE(status_flags, parachute_unhealthy, ActionOptions(fromParachuteActParam(_param_com_parachute_act.get()))); // Battery low failsafe // If battery was low and arming was allowed through COM_ARM_BAT_MIN, don't failsafe immediately for the current low battery warning state diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index d5b3f0a79e..e0bd61b71b 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -157,6 +157,12 @@ private: Return_mode = 3 }; + enum class parachute_unhealthy_failsafe_mode : int32_t { + Warning = 0, + Land = 1, + Return = 2, + }; + static ActionOptions fromNavDllOrRclActParam(int param_value); static ActionOptions fromGfActParam(int param_value); @@ -168,6 +174,7 @@ private: static ActionOptions fromHighWindLimitActParam(int param_value); static ActionOptions fromPosLowActParam(int param_value); static ActionOptions fromRemainingFlightTimeLowActParam(int param_value); + static ActionOptions fromParachuteActParam(int param_value); static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter); @@ -209,7 +216,8 @@ private: (ParamInt) _param_com_qc_act, (ParamInt) _param_com_wind_max_act, (ParamInt) _param_com_fltt_low_act, - (ParamInt) _param_com_pos_low_act + (ParamInt) _param_com_pos_low_act, + (ParamInt) _param_com_parachute_act ); };