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
);
};