From fea29feee683a70abdacea9bb1c575e1adcc8c18 Mon Sep 17 00:00:00 2001 From: gguidone Date: Thu, 9 Apr 2026 15:38:09 +0200 Subject: [PATCH] feat(commander): add Remote ID in-flight failsafe Extend COM_ARM_ODID into a unified arming + in-flight failsafe parameter (0 = Disabled, 1 = Warning, 2 = Return, 3 = Land, 4 = Terminate) Values >= 2 block arming and trigger the configured action if Remote ID is lost while airborne. --- msg/FailsafeFlags.msg | 1 + .../checks/openDroneIDCheck.cpp | 26 +++++++------- src/modules/commander/commander_params.yaml | 17 ++++++--- src/modules/commander/failsafe/failsafe.cpp | 36 +++++++++++++++++++ src/modules/commander/failsafe/failsafe.h | 12 ++++++- 5 files changed, 73 insertions(+), 19 deletions(-) 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 ); };