feat(commander): add failsafe for traffic avoidance system (#27887)

* feat(commander): add traffic avoidance system failsafe

* test(commander): add tests for traffic avoidance failsafe

* docs(parameters): add COM_ARM_TRAFF migration notes

* fix(failsafe): use a shared header instead of hard-coding enum literals

* fix(commander): clear stale traffic avoidance unhealthy flag when check disabled

* refactor(failsafe): rename failsafe_mode_params.h to failsafe_action_modes.h

* docs(adsb_flarm): fix formatting in traffic avoidance parameter table

---------

Co-authored-by: Claudio Chies <chiesc@chies.com>
This commit is contained in:
Anil Kircaliali
2026-07-23 13:26:11 -07:00
committed by GitHub
parent b14e63019b
commit 03bf4a5e95
16 changed files with 353 additions and 38 deletions

View File

@@ -409,7 +409,7 @@ That means DAA will not automatically switch a manually flown vehicle into Hold,
The commander health check reads [`detect_and_avoid_most_urgent`](../msg_docs/DetectAndAvoidMostUrgent.md) and applies the result only while the vehicle is disarmed.
This section covers conflict-based DAA behavior when traffic is already present.
The separate traffic-system-presence arming check is configured by [COM_ARM_TRAFF](../advanced_config/parameter_reference.md#COM_ARM_TRAFF) and described in [ADS-B/FLARM/UTM Receivers > Arming Check](../peripherals/adsb_flarm.md#arming-check).
The separate traffic-system-presence arming check and failsafe is configured by [COM_TRAFF_AVOID](../advanced_config/parameter_reference.md#COM_TRAFF_AVOID) and described in [ADS-B/FLARM/UTM Receivers > Arming Check and Failsafe](../peripherals/adsb_flarm.md#arming-check-and-failsafe).
Preflight behavior:

View File

@@ -176,21 +176,21 @@ These parameters use the same action scale:
Most users can start with the default F3442 volume parameters and tune them only if needed.
See [Detect and Avoid > F3442 Mode](../advanced_features/detect_and_avoid.md#f3442-mode), which also includes the zone-computation equations.
### Arming Check
### Arming Check and Failsafe
PX4 can be configured to check for the presence of a traffic avoidance system (for example an ADS-B or FLARM receiver) before arming.
This ensures that a traffic avoidance system is connected and functioning before flight.
PX4 can be configured to warn about, or require, a traffic avoidance system (for example an ADS-B or FLARM receiver), and to trigger a failsafe if the system stops sending MAVLink heartbeats in flight.
This check only verifies that a traffic source is present. It is separate from DAA rejecting arming because active traffic already requires an automatic action; that behavior is described in [Detect and Avoid > Arming, Preflight, and Ground Behavior](../advanced_features/detect_and_avoid.md#arming-preflight-and-ground-behavior).
The check is configured using the [COM_ARM_TRAFF](../advanced_config/parameter_reference.md#COM_ARM_TRAFF) parameter:
The behavior is configured using the [COM_TRAFF_AVOID](../advanced_config/parameter_reference.md#COM_TRAFF_AVOID) parameter:
| Value | Description |
| ----- | -------------------------------------------------------------------------------------------------------------------------- |
| 0 | Disabled (default). No check is performed. |
| 1 | Warning only. A warning is issued if no traffic avoidance system is detected, but arming is allowed. |
| 2 | Enforce for all modes. Arming is denied if no traffic avoidance system is detected, regardless of flight mode. |
| 3 | Enforce for mission modes only. Arming is denied if no traffic avoidance system is detected and a mission mode is planned. |
| Value | Description|
| ----- | ---------- |
| 0 | Disabled (default). No check is performed.|
| 1 | Warning (allow arming). Arming is allowed even if no traffic avoidance system is detected; a warning is issued if it is missing or lost in flight. |
| 2 | Error (block arming). Arming is denied while no traffic avoidance system is detected, and a warning is issued if it is lost in flight. |
| 3 | Return. Arming is denied while no traffic avoidance system is detected, and the vehicle returns if it is lost in flight.|
| 4 | Land. Arming is denied while no traffic avoidance system is detected, and the vehicle lands if it is lost in flight.|
When a traffic avoidance system is detected, the system tracks its presence with a 3-second timeout.
If the system is lost or regained, corresponding events are logged ("Traffic avoidance system lost" / "Traffic avoidance system regained").

View File

@@ -30,6 +30,8 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
## Upgrade Guide
- `COM_ARM_TRAFF` has been replaced by `COM_TRAFF_AVOID`. The old value 3 ("enforce for mission modes only") is migrated to `COM_TRAFF_AVOID=2`, which blocks arming in all modes, not just mission modes. If you relied on being able to arm manually with traffic detected, set `COM_TRAFF_AVOID=1` (warning only) instead.
## Other changes
- Fast mission Return modes ([RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) = 2 and 4) now skip `DO_JUMP` commands (loops) while following the mission path. ([PX4-Autopilot#26993: fix(navigator): goToNextPositionItem skip loops when required](https://github.com/PX4/PX4-Autopilot/pull/26993))

View File

@@ -60,5 +60,6 @@ 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 traffic_avoidance_unhealthy # Traffic avoidance (ADS-B/FLARM) system missing or unhealthy
bool remote_id_unhealthy # Remote ID (Open Drone ID) system missing or unhealthy
bool gnss_lost # Active GNSS count dropped below SYS_HAS_NUM_GNSS, or two receivers report inconsistent positions

View File

@@ -286,5 +286,27 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
}
}
// 2026-07-13: translate COM_ARM_TRAFF (arming check only) to COM_TRAFF_AVOID (arming check + failsafe action)
{
if ((node->type == bson_type_t::BSON_INT32) && (strcmp("COM_ARM_TRAFF", node->name) == 0)) {
// old: 0 Disabled, 1 Warning only (arming allowed), 2 Enforce all modes, 3 Enforce mission only
// new: 0 Disabled, 1 Warning (arming allowed), 2 Error (arming blocked)
// Old value 3 (mission-only enforcement) is intentionally mapped to 2 (all modes): mode-scoped
// arming enforcement is no longer supported, so we err on the restrictive side.
// COM_ARM_TRAFF never triggered an in-flight failsafe action, so the new failsafe
// action defaults to Warning either way; only the arming behavior is preserved.
if (node->i32 == 1) {
node->i32 = 1;
} else if (node->i32 >= 2) {
node->i32 = 2;
}
strcpy(node->name, "COM_TRAFF_AVOID");
PX4_INFO("migrating %s -> %s", "COM_ARM_TRAFF", "COM_TRAFF_AVOID");
return param_modify_on_import_ret::PARAM_MODIFIED;
}
}
return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
}

View File

@@ -3072,7 +3072,7 @@ void Commander::dataLinkCheck()
}
// Traffic avoidance system (ADSB/FLARM)
if ((_param_com_arm_traff.get() > 0) && (hrt_elapsed_time(&_datalink_last_heartbeat_traffic_avoidance_system) > 3_s)
if ((hrt_elapsed_time(&_datalink_last_heartbeat_traffic_avoidance_system) > 3_s)
&& !_traffic_avoidance_system_lost) {
mavlink_log_critical(&_mavlink_log_pub, "Traffic avoidance system lost\t");
events::send(events::ID("commander_traffic_avoidance_lost"), events::Log::Critical, "Traffic avoidance system lost");

View File

@@ -350,7 +350,6 @@ private:
(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max,
(ParamBool<px4::params::COM_ARM_ON_BOOT>) _param_com_arm_on_boot,
(ParamInt<px4::params::COM_FLTMODE_BOOT>) _param_com_fltmode_boot,
(ParamInt<px4::params::COM_ARM_TRAFF>) _param_com_arm_traff,
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act
)
};

View File

@@ -90,3 +90,5 @@ px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
)
px4_add_functional_gtest(SRC gnssRedundancyChecksTest.cpp LINKLIBS health_and_arming_checks mode_util)
px4_add_functional_gtest(SRC trafficAvoidanceCheckTest.cpp LINKLIBS health_and_arming_checks mode_util)

View File

@@ -33,39 +33,49 @@
#include "trafficAvoidanceCheck.hpp"
#include "../../failsafe/failsafe_action_modes.h"
void TrafficAvoidanceChecks::checkAndReport(const Context &context, Report &reporter)
{
NavModes affected_modes{NavModes::None}; // COM_ARM_TRAFF 1 - warning only, arming allowed, affected_modes stays None
const auto mode = static_cast<traffic_avoidance::FailsafeMode>(_param_com_traff_avoid.get());
const bool enabled = traffic_avoidance::isEnabled(mode);
switch (_param_com_arm_traff.get()) {
case 0:
return; // Check disabled
// Always update the flag, so that disabling the check also clears a previously-set flag
reporter.failsafeFlags().traffic_avoidance_unhealthy = enabled
&& !context.status().traffic_avoidance_system_present;
case 2:
affected_modes = NavModes::All; // Disallow arming for all modes
break;
case 3:
affected_modes = NavModes::Mission; // Disallow arming for mission
break;
if (!enabled) {
return;
}
if (!context.status().traffic_avoidance_system_present) {
const bool block_arming = traffic_avoidance::blocksArming(mode);
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
/* EVENT
* @description
* Traffic avoidance system (ADSB/FLARM) failed to report. Make sure it is setup and connected properly.
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_TRAFF</param> parameter.
* Configured by <param>COM_TRAFF_AVOID</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(affected_modes, health_component_t::traffic_avoidance,
events::ID("check_traffic_avoidance_missing"),
events::Log::Error, "Traffic avoidance system missing");
reporter.healthFailure(nav_modes, health_component_t::traffic_avoidance,
events::ID("check_traffic_avoidance_missing"),
log_level, "Traffic avoidance system missing");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Traffic avoidance system missing");
if (block_arming) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Traffic avoidance system missing");
} else {
mavlink_log_warning(reporter.mavlink_log_pub(), "Traffic avoidance system missing");
}
}
}
if (context.status().traffic_avoidance_system_present) {
reporter.setIsPresent(health_component_t::traffic_avoidance);
}
}

View File

@@ -45,6 +45,6 @@ public:
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::COM_ARM_TRAFF>) _param_com_arm_traff
(ParamInt<px4::params::COM_TRAFF_AVOID>) _param_com_traff_avoid
)
};

View File

@@ -0,0 +1,147 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include "checks/trafficAvoidanceCheck.hpp"
#include "../failsafe/failsafe_action_modes.h"
#include <px4_platform_common/param.h>
// to run: make tests TESTFILTER=trafficAvoidanceCheck
/* EVENT
* @skip-file
*/
class TrafficAvoidanceChecksTest : public ::testing::Test
{
public:
void SetUp() override
{
param_control_autosave(false);
param_reset(param_find("COM_TRAFF_AVOID"));
}
// Run the check and store results in _failsafe_flags and the health/arming result caches.
void runCheck(bool system_present)
{
vehicle_status_s status{};
status.traffic_avoidance_system_present = system_present;
_check.updateParams();
Context context{status};
// _failsafe_flags deliberately persists across calls, like commander's failsafe_flags in production
Report reporter{_failsafe_flags, 0};
_check.checkAndReport(context, reporter);
_health_error_traffic_avoidance = (bool)(reporter.healthResults().error & health_component_t::traffic_avoidance);
_health_warning_traffic_avoidance = (bool)(reporter.healthResults().warning & health_component_t::traffic_avoidance);
_is_present_traffic_avoidance = (bool)(reporter.healthResults().is_present & health_component_t::traffic_avoidance);
_can_arm_all_modes = reporter.armingCheckResults().can_arm == NavModes::All;
}
void setParam(traffic_avoidance::FailsafeMode mode)
{
int32_t com_traff_avoid = static_cast<int32_t>(mode);
param_set(param_find("COM_TRAFF_AVOID"), &com_traff_avoid);
}
failsafe_flags_s _failsafe_flags{};
bool _health_error_traffic_avoidance{false};
bool _health_warning_traffic_avoidance{false};
bool _is_present_traffic_avoidance{false};
bool _can_arm_all_modes{false};
TrafficAvoidanceChecks _check;
};
// COM_TRAFF_AVOID = 0 (Disabled): missing system is ignored entirely, arming unaffected.
TEST_F(TrafficAvoidanceChecksTest, DisabledIgnoresMissingSystem)
{
setParam(traffic_avoidance::FailsafeMode::Disabled);
runCheck(false);
EXPECT_FALSE(_failsafe_flags.traffic_avoidance_unhealthy);
EXPECT_FALSE(_health_error_traffic_avoidance);
EXPECT_FALSE(_health_warning_traffic_avoidance);
EXPECT_TRUE(_can_arm_all_modes);
}
// COM_TRAFF_AVOID = 1 (Warning): missing system sets unhealthy + a warning, but does NOT block arming.
TEST_F(TrafficAvoidanceChecksTest, WarningMissingSystemDoesNotBlockArming)
{
setParam(traffic_avoidance::FailsafeMode::Warning);
runCheck(false);
EXPECT_TRUE(_failsafe_flags.traffic_avoidance_unhealthy);
EXPECT_TRUE(_health_warning_traffic_avoidance);
EXPECT_FALSE(_health_error_traffic_avoidance);
EXPECT_TRUE(_can_arm_all_modes);
}
// COM_TRAFF_AVOID = 2 (Error): missing system sets unhealthy + an error, and blocks arming.
TEST_F(TrafficAvoidanceChecksTest, ErrorMissingSystemBlocksArming)
{
setParam(traffic_avoidance::FailsafeMode::Error);
runCheck(false);
EXPECT_TRUE(_failsafe_flags.traffic_avoidance_unhealthy);
EXPECT_TRUE(_health_error_traffic_avoidance);
EXPECT_FALSE(_can_arm_all_modes);
}
// Disabling the check (e.g. in flight, to allow arming) clears a previously-set unhealthy flag.
TEST_F(TrafficAvoidanceChecksTest, DisablingClearsStaleUnhealthyFlag)
{
setParam(traffic_avoidance::FailsafeMode::Warning);
runCheck(false);
EXPECT_TRUE(_failsafe_flags.traffic_avoidance_unhealthy);
setParam(traffic_avoidance::FailsafeMode::Disabled);
runCheck(false);
EXPECT_FALSE(_failsafe_flags.traffic_avoidance_unhealthy);
EXPECT_TRUE(_can_arm_all_modes);
}
// COM_TRAFF_AVOID >= 1 and system present -> healthy, present, arming unaffected.
TEST_F(TrafficAvoidanceChecksTest, PresentSystemIsHealthy)
{
setParam(traffic_avoidance::FailsafeMode::Error);
runCheck(true);
EXPECT_FALSE(_failsafe_flags.traffic_avoidance_unhealthy);
EXPECT_FALSE(_health_error_traffic_avoidance);
EXPECT_FALSE(_health_warning_traffic_avoidance);
EXPECT_TRUE(_is_present_traffic_avoidance);
EXPECT_TRUE(_can_arm_all_modes);
}

View File

@@ -575,19 +575,20 @@ parameters:
4: Land
5: Terminate
default: 0
COM_ARM_TRAFF:
COM_TRAFF_AVOID:
description:
short: Enable Traffic Avoidance system detection check
short: Traffic avoidance system requirement and failsafe
long: |-
This check detects if a traffic avoidance system (ADSB/FLARM transponder)
is missing. Depending on the value of the parameter, the check can be
disabled, warn only, or deny arming.
Warn about, and optionally require, a traffic avoidance system (ADS-B/FLARM
transponder, detected via MAVLink heartbeats with a 3 second timeout), and
trigger a failsafe action when it is missing or lost in flight.
type: enum
values:
0: Disabled
1: Warning only
2: Enforce for all modes
3: Enforce for mission modes only
1: Warning
2: Error
3: Return
4: Land
default: 0
COM_SPOOLUP_TIME:
description:

View File

@@ -32,6 +32,7 @@
****************************************************************************/
#include "failsafe.h"
#include "failsafe_action_modes.h"
#include <px4_platform_common/log.h>
#include <uORB/topics/vehicle_status.h>
@@ -444,6 +445,36 @@ FailsafeBase::ActionOptions Failsafe::fromParachuteActParam(int param_value)
return options;
}
FailsafeBase::ActionOptions Failsafe::fromTrafficAvoidanceActParam(int param_value)
{
ActionOptions options{};
switch (static_cast<traffic_avoidance::FailsafeMode>(param_value)) {
case traffic_avoidance::FailsafeMode::Disabled:
default:
options.action = Action::None;
break;
case traffic_avoidance::FailsafeMode::Warning:
case traffic_avoidance::FailsafeMode::Error:
options.action = Action::Warn;
options.clear_condition = ClearCondition::WhenConditionClears;
break;
case traffic_avoidance::FailsafeMode::Return:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case traffic_avoidance::FailsafeMode::Land:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value)
{
ActionOptions options{};
@@ -641,6 +672,9 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
// Parachute system health failsafe
CHECK_FAILSAFE(status_flags, parachute_unhealthy, ActionOptions(fromParachuteActParam(_param_com_parachute.get())));
// Traffic avoidance system health failsafe
CHECK_FAILSAFE(status_flags, traffic_avoidance_unhealthy, ActionOptions(fromTrafficAvoidanceActParam(_param_com_traff_avoid.get())));
// 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()));

View File

@@ -187,6 +187,7 @@ private:
static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
static ActionOptions fromOdidFailActParam(int param_value);
static ActionOptions fromParachuteActParam(int param_value);
static ActionOptions fromTrafficAvoidanceActParam(int param_value);
static ActionOptions fromGnssLossActParam(int param_value);
static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
@@ -232,6 +233,7 @@ private:
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act,
(ParamInt<px4::params::COM_ARM_ODID>) _param_com_arm_odid,
(ParamInt<px4::params::COM_PARACHUTE>) _param_com_parachute,
(ParamInt<px4::params::COM_TRAFF_AVOID>) _param_com_traff_avoid,
(ParamInt<px4::params::COM_GNSSLOSS_ACT>) _param_com_gnssloss_act
);

View File

@@ -0,0 +1,60 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <stdint.h>
// Failsafe action-mode enums (values must match the parameter metadata
// in commander_params.yaml), one namespace per failsafe. Kept in this
// dependency-free header so the enum and its severity predicates can be
// shared between the failsafe state machine and the HealthAndArmingChecks
// without the latter pulling in the failsafe framework.
namespace traffic_avoidance
{
// COM_TRAFF_AVOID parameter values.
enum class FailsafeMode : int32_t {
Disabled = 0,
Warning = 1, // arming allowed, in-flight warning
Error = 2, // arming blocked, in-flight warning
Return = 3, // arming blocked, in-flight RTL
Land = 4, // arming blocked, in-flight Land
};
// Increasing order of severity in enum is assumed.
constexpr bool isEnabled(FailsafeMode mode) { return mode >= FailsafeMode::Warning; }
constexpr bool blocksArming(FailsafeMode mode) { return mode >= FailsafeMode::Error; }
} // namespace traffic_avoidance

View File

@@ -34,6 +34,7 @@
#include <gtest/gtest.h>
#include "failsafe.h"
#include "failsafe_action_modes.h"
#include <uORB/topics/vehicle_status.h>
#include "../ModeUtil/mode_requirements.hpp"
@@ -728,6 +729,40 @@ TEST_F(FailsafeTest, FallbackAltitudeUsesNavRclActParam)
EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
}
TEST_F(FailsafeTest, TrafficAvoidanceUnhealthyUsesTrafficAvoidActParam)
{
// Each param value is exercised on its own fresh Failsafe instance, to avoid
// action hysteresis/clear-conditions from one value leaking into the next.
auto selectedActionFor = [](traffic_avoidance::FailsafeMode mode) {
int32_t com_traff_avoid = static_cast<int32_t>(mode);
param_set(param_handle(px4::params::COM_TRAFF_AVOID), &com_traff_avoid);
// Disable the generic user-takeover hold delay (set to 5s in SetUp()) so a newly
// triggered RTL/Land is selected immediately instead of Hold-then-RTL/Land.
float com_fail_act_t = 0.f;
param_set(param_handle(px4::params::COM_FAIL_ACT_T), &com_fail_act_t);
Failsafe failsafe(nullptr);
failsafe_flags_s failsafe_flags{};
mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags);
failsafe_flags.traffic_avoidance_unhealthy = true;
FailsafeBase::State state{};
state.armed = true;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
failsafe.update(5_s, state, false, false, failsafe_flags);
return failsafe.selectedAction();
};
EXPECT_EQ(selectedActionFor(traffic_avoidance::FailsafeMode::Disabled), FailsafeBase::Action::None);
EXPECT_EQ(selectedActionFor(traffic_avoidance::FailsafeMode::Warning), FailsafeBase::Action::Warn);
EXPECT_EQ(selectedActionFor(traffic_avoidance::FailsafeMode::Error), FailsafeBase::Action::Warn); // same as Warning in-flight
EXPECT_EQ(selectedActionFor(traffic_avoidance::FailsafeMode::Return), FailsafeBase::Action::RTL);
EXPECT_EQ(selectedActionFor(traffic_avoidance::FailsafeMode::Land), FailsafeBase::Action::Land);
}
TEST_F(FailsafeTest, FallbackStabilizedRequiresManualControl)
{
int nav_rcl_act = 2;