mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-25 13:57:42 +08:00
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:
@@ -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:
|
||||
|
||||
|
||||
@@ -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").
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -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
|
||||
)
|
||||
};
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
)
|
||||
};
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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:
|
||||
|
||||
@@ -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()));
|
||||
|
||||
@@ -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
|
||||
);
|
||||
|
||||
|
||||
60
src/modules/commander/failsafe/failsafe_action_modes.h
Normal file
60
src/modules/commander/failsafe/failsafe_action_modes.h
Normal 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
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user