diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg
index 2d974791f7..545ce85a2f 100644
--- a/msg/FailsafeFlags.msg
+++ b/msg/FailsafeFlags.msg
@@ -59,3 +59,4 @@ bool flight_time_limit_exceeded # Maximum flight time exceeded
bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid
bool navigator_failure # Navigator failed to execute a mode
bool parachute_unhealthy # Parachute system missing or unhealthy
+bool remote_id_unhealthy # Remote ID (Open Drone ID) system missing or unhealthy
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp
index bb4847a2c5..5a847d40ec 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp
@@ -36,17 +36,17 @@
void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
{
- // Check to see if the check has been disabled
+ reporter.failsafeFlags().remote_id_unhealthy =
+ !context.status().open_drone_id_system_present ||
+ !context.status().open_drone_id_system_healthy;
+
if (!_param_com_arm_odid.get()) {
return;
}
- NavModes affected_modes{NavModes::None};
-
- if (_param_com_arm_odid.get() == 2) {
- // disallow arming without the Open Drone ID system
- affected_modes = NavModes::All;
- }
+ const bool is_error = _param_com_arm_odid.get() >= 2;
+ const NavModes affected_modes = is_error ? NavModes::All : NavModes::None;
+ const events::Log log_level = is_error ? events::Log::Error : events::Log::Warning;
if (!context.status().open_drone_id_system_present) {
/* EVENT
@@ -57,9 +57,9 @@ void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
* This check can be configured via COM_ARM_ODID parameter.
*
*/
- reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
- events::ID("check_open_drone_id_missing"),
- events::Log::Error, "Open Drone ID system missing");
+ reporter.healthFailure(affected_modes, health_component_t::open_drone_id,
+ events::ID("check_open_drone_id_missing"),
+ log_level, "Open Drone ID system missing");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system missing");
@@ -74,9 +74,9 @@ void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
* This check can be configured via COM_ARM_ODID parameter.
*
*/
- reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
- events::ID("check_open_drone_id_unhealthy"),
- events::Log::Error, "Open Drone ID system not ready");
+ reporter.healthFailure(affected_modes, health_component_t::open_drone_id,
+ events::ID("check_open_drone_id_unhealthy"),
+ log_level, "Open Drone ID system not ready");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system not ready");
diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml
index 0352265131..333cdfb4b8 100644
--- a/src/modules/commander/commander_params.yaml
+++ b/src/modules/commander/commander_params.yaml
@@ -612,17 +612,24 @@ parameters:
default: 1
COM_ARM_ODID:
description:
- short: Enable Drone ID system detection and health check
+ short: Open Drone ID system check and in-flight failsafe
long: |-
- This check detects if the Open Drone ID system is missing.
- Depending on the value of the parameter, the check can be
- disabled, warn only or deny arming.
+ Configures both the arming check and the in-flight failsafe for the
+ Open Drone ID (Remote ID) system.
+
+ Warning: warns at arming but does not block it, and takes no in-flight action.
+ Land / Return / Terminate: block arming without the ODID system present and
+ healthy, and trigger the corresponding failsafe action if the system is lost
+ while airborne (heartbeat timeout > 3 s).
type: enum
values:
0: Disabled
1: Warning only
- 2: Enforce Open Drone ID system presence
+ 2: Return
+ 3: Land
+ 4: Terminate
default: 0
+ increment: 1
COM_ARM_TRAFF:
description:
short: Enable Traffic Avoidance system detection check
diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp
index 25d9728cc6..648874dcba 100644
--- a/src/modules/commander/failsafe/failsafe.cpp
+++ b/src/modules/commander/failsafe/failsafe.cpp
@@ -442,6 +442,37 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par
return options;
}
+FailsafeBase::ActionOptions Failsafe::fromOdidFailActParam(int param_value)
+{
+ ActionOptions options{};
+
+ switch (open_drone_id_failsafe_mode(param_value)) {
+ case open_drone_id_failsafe_mode::Return_mode:
+ options.action = Action::RTL;
+ options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+ break;
+
+ case open_drone_id_failsafe_mode::Land_mode:
+ options.action = Action::Land;
+ options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+ break;
+
+ case open_drone_id_failsafe_mode::Terminate:
+ options.allow_user_takeover = UserTakeoverAllowed::Never;
+ options.action = Action::Terminate;
+ options.clear_condition = ClearCondition::Never;
+ break;
+
+ case open_drone_id_failsafe_mode::Disabled:
+ case open_drone_id_failsafe_mode::Warning:
+ default:
+ options.action = Action::None;
+ break;
+ }
+
+ return options;
+}
+
bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter)
{
switch (user_intended_mode) {
@@ -577,6 +608,11 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
// Parachute system health failsafe
CHECK_FAILSAFE(status_flags, parachute_unhealthy, Action::RTL);
+ // Remote ID (Open Drone ID) loss failsafe
+ if (state.armed && _param_com_arm_odid.get() >= int32_t(open_drone_id_failsafe_mode::Return_mode)) {
+ CHECK_FAILSAFE(status_flags, remote_id_unhealthy, fromOdidFailActParam(_param_com_arm_odid.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
const bool warning_worse_than_at_arming = (status_flags.battery_warning > _battery_warning_at_arming);
diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h
index d5b3f0a79e..3f0ac3b41d 100644
--- a/src/modules/commander/failsafe/failsafe.h
+++ b/src/modules/commander/failsafe/failsafe.h
@@ -157,6 +157,14 @@ private:
Return_mode = 3
};
+ enum class open_drone_id_failsafe_mode : int32_t {
+ Disabled = 0,
+ Warning = 1,
+ Return_mode = 2,
+ Land_mode = 3,
+ Terminate = 4,
+ };
+
static ActionOptions fromNavDllOrRclActParam(int param_value);
static ActionOptions fromGfActParam(int param_value);
@@ -168,6 +176,7 @@ private:
static ActionOptions fromHighWindLimitActParam(int param_value);
static ActionOptions fromPosLowActParam(int param_value);
static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
+ static ActionOptions fromOdidFailActParam(int param_value);
static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
@@ -209,7 +218,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_arm_odid
);
};