Skip to content

Commit 25b24c8

Browse files
gguidonegguidone
committed
feat(commander): add COM_PARA_ACT to configure parachute unhealthy failsafe
Previously the parachute unhealthy failsafe was hardcoded to RTL.
1 parent b48f3ef commit 25b24c8

5 files changed

Lines changed: 107 additions & 41 deletions

File tree

‎src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp‎

Lines changed: 51 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -38,49 +38,64 @@ using namespace time_literals;
3838

3939
void ParachuteChecks::checkAndReport(const Context &context, Report &reporter)
4040
{
41-
if (!_param_com_parachute.get()) {
42-
return;
43-
}
41+
const int action = _param_com_parachute.get();
4442

45-
reporter.failsafeFlags().parachute_unhealthy =
46-
!context.status().parachute_system_present ||
47-
!context.status().parachute_system_healthy;
43+
if (action > 0) {
44+
reporter.failsafeFlags().parachute_unhealthy =
45+
!context.status().parachute_system_present ||
46+
!context.status().parachute_system_healthy;
4847

49-
if (!context.status().parachute_system_present) {
50-
/* EVENT
51-
* @description
52-
* No MAVLink parachute heartbeat detected. Check connection, power, configuration.
53-
*
54-
* <profile name="dev">
55-
* Enabled by <param>COM_PARACHUTE</param>
56-
* </profile>
57-
*/
58-
reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_missing"),
59-
events::Log::Error, "Parachute system missing");
48+
// Values >= 2 block arming; value 1 is warning-only
49+
const bool warn_only = (action == 1);
50+
const events::Log log_level = warn_only ? events::Log::Warning : events::Log::Error;
51+
const NavModes affected_modes = warn_only ? NavModes::None : NavModes::All;
6052

61-
if (reporter.mavlink_log_pub()) {
62-
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system missing");
63-
}
53+
if (!context.status().parachute_system_present) {
54+
/* EVENT
55+
* @description
56+
* No MAVLink parachute heartbeat detected. Check connection, power, configuration.
57+
*
58+
* <profile name="dev">
59+
* Configured by <param>COM_PARACHUTE</param>
60+
* </profile>
61+
*/
62+
reporter.healthFailure(affected_modes, health_component_t::parachute, events::ID("check_parachute_missing"),
63+
log_level, "Parachute system missing");
64+
65+
if (reporter.mavlink_log_pub()) {
66+
if (warn_only) {
67+
mavlink_log_warning(reporter.mavlink_log_pub(), "Parachute system missing");
68+
69+
} else {
70+
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system missing");
71+
}
72+
}
6473

65-
} else if (!context.status().parachute_system_healthy) {
74+
} else if (!context.status().parachute_system_healthy) {
6675

67-
/* EVENT
68-
* @description
69-
* MAVLink parachute system reports unhealthy status.
70-
*
71-
* <profile name="dev">
72-
* Enabled by <param>COM_PARACHUTE</param>
73-
* </profile>
74-
*/
75-
reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_unhealthy"),
76-
events::Log::Error, "Parachute system not ready");
76+
/* EVENT
77+
* @description
78+
* MAVLink parachute system reports unhealthy status.
79+
*
80+
* <profile name="dev">
81+
* Configured by <param>COM_PARACHUTE</param>
82+
* </profile>
83+
*/
84+
reporter.healthFailure(affected_modes, health_component_t::parachute, events::ID("check_parachute_unhealthy"),
85+
log_level, "Parachute system not ready");
7786

78-
if (reporter.mavlink_log_pub()) {
79-
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system not ready");
87+
if (reporter.mavlink_log_pub()) {
88+
if (warn_only) {
89+
mavlink_log_warning(reporter.mavlink_log_pub(), "Parachute system not ready");
90+
91+
} else {
92+
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system not ready");
93+
}
94+
}
8095
}
81-
}
8296

83-
if (context.status().parachute_system_present) {
84-
reporter.setIsPresent(health_component_t::parachute);
97+
if (context.status().parachute_system_present) {
98+
reporter.setIsPresent(health_component_t::parachute);
99+
}
85100
}
86101
}

‎src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,6 @@ class ParachuteChecks : public HealthAndArmingCheckBase
4545

4646
private:
4747
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
48-
(ParamBool<px4::params::COM_PARACHUTE>) _param_com_parachute
48+
(ParamInt<px4::params::COM_PARACHUTE>) _param_com_parachute
4949
)
5050
};

‎src/modules/commander/commander_params.yaml‎

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -510,9 +510,20 @@ parameters:
510510
max: 3
511511
COM_PARACHUTE:
512512
description:
513-
short: Require MAVLink parachute system to be present and healthy
514-
type: boolean
513+
short: Parachute system monitoring and failsafe action
514+
long: |-
515+
Configures whether a MAVLink parachute system is monitored and what action is
516+
taken when it reports missing or unhealthy status.
517+
From Error onwards the check also blocks arming.
518+
type: enum
519+
values:
520+
0: Disabled
521+
1: Warning only
522+
2: Error (blocks arming)
523+
3: Return
524+
4: Land
515525
default: 0
526+
increment: 1
516527
COM_ARM_CHK_ESCS:
517528
description:
518529
short: Enable checks on ESCs that report telemetry

‎src/modules/commander/failsafe/failsafe.cpp‎

Lines changed: 31 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -412,6 +412,36 @@ FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value)
412412
return options;
413413
}
414414

415+
FailsafeBase::ActionOptions Failsafe::fromParachuteActParam(int param_value)
416+
{
417+
ActionOptions options{};
418+
419+
switch (parachute_unhealthy_failsafe_mode(param_value)) {
420+
case parachute_unhealthy_failsafe_mode::Disabled:
421+
default:
422+
options.action = Action::None;
423+
break;
424+
425+
case parachute_unhealthy_failsafe_mode::Warning:
426+
case parachute_unhealthy_failsafe_mode::Error:
427+
options.action = Action::Warn;
428+
options.clear_condition = ClearCondition::WhenConditionClears;
429+
break;
430+
431+
case parachute_unhealthy_failsafe_mode::Return:
432+
options.action = Action::RTL;
433+
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
434+
break;
435+
436+
case parachute_unhealthy_failsafe_mode::Land:
437+
options.action = Action::Land;
438+
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
439+
break;
440+
}
441+
442+
return options;
443+
}
444+
415445
FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value)
416446
{
417447
ActionOptions options{};
@@ -575,7 +605,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
575605
}
576606

577607
// Parachute system health failsafe
578-
CHECK_FAILSAFE(status_flags, parachute_unhealthy, Action::RTL);
608+
CHECK_FAILSAFE(status_flags, parachute_unhealthy, ActionOptions(fromParachuteActParam(_param_com_parachute_act.get())));
579609

580610
// Battery low failsafe
581611
// If battery was low and arming was allowed through COM_ARM_BAT_MIN, don't failsafe immediately for the current low battery warning state

‎src/modules/commander/failsafe/failsafe.h‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -157,6 +157,14 @@ class Failsafe : public FailsafeBase
157157
Return_mode = 3
158158
};
159159

160+
enum class parachute_unhealthy_failsafe_mode : int32_t {
161+
Disabled = 0,
162+
Warning = 1,
163+
Error = 2,
164+
Return = 3,
165+
Land = 4,
166+
};
167+
160168
static ActionOptions fromNavDllOrRclActParam(int param_value);
161169

162170
static ActionOptions fromGfActParam(int param_value);
@@ -168,6 +176,7 @@ class Failsafe : public FailsafeBase
168176
static ActionOptions fromHighWindLimitActParam(int param_value);
169177
static ActionOptions fromPosLowActParam(int param_value);
170178
static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
179+
static ActionOptions fromParachuteActParam(int param_value);
171180

172181
static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
173182

@@ -209,7 +218,8 @@ class Failsafe : public FailsafeBase
209218
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
210219
(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
211220
(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act,
212-
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
221+
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act,
222+
(ParamInt<px4::params::COM_PARACHUTE>) _param_com_parachute_act
213223
);
214224

215225
};

0 commit comments

Comments
 (0)