Skip to content

fix(commander): failsafe prevent manual fallback without RC #27408

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
AkaiEurus wants to merge 2 commits into
base: main
Choose a base branch
from
+109 −8
Open

fix(commander): failsafe prevent manual fallback without RC #27408

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1cfa312
failsafe: prevent manual fallback without RC
AkaiEurus May 20, 2026
4593153
fix(commander): consolidate manual fallback RC loss handling
AkaiEurus May 21, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 11 additions & 8 deletions src/modules/commander/failsafe/failsafe.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,13 @@

using namespace time_literals;

static bool manualControlFallbackAction(FailsafeBase::Action action)
{
return action == FailsafeBase::Action::FallbackPosCtrl
|| action == FailsafeBase::Action::FallbackAltCtrl
|| action == FailsafeBase::Action::FallbackStab;
}

FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
{
ActionOptions options{};
Expand Down Expand Up @@ -729,14 +736,6 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_
return action;
}

if (action == Action::FallbackPosCtrl || action == Action::FallbackAltCtrl || action == Action::FallbackStab) {
// Check if RC is available, if not use the mode specified in NAV_RCL_ACT
if (status_flags.manual_control_signal_lost) {
ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get());
action = rc_loss_action.action;
}

}
}

// PosCtrl/PositionSlow -> AltCtrl
Expand All @@ -754,6 +753,10 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
}

if (status_flags.manual_control_signal_lost && manualControlFallbackAction(action)) {
action = manualControlLossFallbackAction();
}

// Last, check can_run for intended mode
if (!modeCanRun(status_flags, user_intended_mode)) {
action = Action::RTL;
Expand Down
2 changes: 2 additions & 0 deletions src/modules/commander/failsafe/failsafe.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,8 @@ class Failsafe : public FailsafeBase

static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);

Action manualControlLossFallbackAction() const { return fromNavDllOrRclActParam(_param_nav_rcl_act.get()).action; }

const int _caller_id_mode_fallback{genCallerId()};
bool _last_state_mode_fallback{false};
const int _caller_id_mission_control_lost{genCallerId()};
Expand Down
96 changes: 96 additions & 0 deletions src/modules/commander/failsafe/failsafe_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

#include <gtest/gtest.h>

#include "failsafe.h"
#include "framework.h"
#include <uORB/topics/vehicle_status.h>
#include "../ModeUtil/mode_requirements.hpp"
Expand Down Expand Up @@ -562,3 +563,98 @@ TEST_F(FailsafeTest, user_termination)
EXPECT_EQ(updated_user_intented_mode, state.user_intended_mode);
EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
}

TEST_F(FailsafeTest, fallback_altitude_requires_manual_control)
Comment thread
AkaiEurus marked this conversation as resolved.
Comment thread
AkaiEurus marked this conversation as resolved.
{
int nav_rcl_act = 2;
param_set(param_handle(px4::params::NAV_RCL_ACT), &nav_rcl_act);

Failsafe failsafe(nullptr);

failsafe_flags_s failsafe_flags{};
mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags);
failsafe_flags.manual_control_signal_lost = true;

FailsafeBase::State state{};
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 5_s;

// If arming was allowed without manual control, RC loss is ignored until manual control returns.
failsafe.update(time, state, false, false, failsafe_flags);

state.armed = true;
time += 10_ms;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);

// Losing position in PosCtl would normally fall back to AltCtl, but AltCtl needs manual control.
failsafe_flags.local_position_invalid_relaxed = true;
time += 10_ms;
failsafe.update(time, state, false, false, failsafe_flags);
EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
Copy link
Copy Markdown
Contributor

@dakejahl dakejahl May 21, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With default NAV_RCL_ACT=2 (Return), this assertion would still hold if the new code hard-coded Action::RTL instead of reading the parameter. Adding a sibling test that sets NAV_RCL_ACT to e.g. Land or Hold and asserts the corresponding action would actually validate the parameter linkage.

}

TEST_F(FailsafeTest, fallback_altitude_uses_nav_rcl_act_param)
Comment thread
AkaiEurus marked this conversation as resolved.
{
int nav_rcl_act = 5;
param_set(param_handle(px4::params::NAV_RCL_ACT), &nav_rcl_act);

Failsafe failsafe(nullptr);

failsafe_flags_s failsafe_flags{};
mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags);
failsafe_flags.manual_control_signal_lost = true;

FailsafeBase::State state{};
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 5_s;

// If arming was allowed without manual control, RC loss is ignored until manual control returns.
failsafe.update(time, state, false, false, failsafe_flags);

state.armed = true;
time += 10_ms;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);

// Losing position in PosCtl triggers the manual-control fallback, which should use NAV_RCL_ACT.
failsafe_flags.local_position_invalid_relaxed = true;
time += 10_ms;
failsafe.update(time, state, false, false, failsafe_flags);
EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
}

TEST_F(FailsafeTest, fallback_stabilized_requires_manual_control)
Comment thread
AkaiEurus marked this conversation as resolved.
Comment thread
AkaiEurus marked this conversation as resolved.
{
int nav_rcl_act = 2;
param_set(param_handle(px4::params::NAV_RCL_ACT), &nav_rcl_act);

Failsafe failsafe(nullptr);

failsafe_flags_s failsafe_flags{};
mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags);
failsafe_flags.manual_control_signal_lost = true;

FailsafeBase::State state{};
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 5_s;

// If arming was allowed without manual control, RC loss is ignored until manual control returns.
failsafe.update(time, state, false, false, failsafe_flags);

state.armed = true;
time += 10_ms;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);

// Losing altitude in AltCtl would normally fall back to Stabilized, but Stabilized needs manual control.
failsafe_flags.local_altitude_invalid = true;
Copy link
Copy Markdown
Contributor

@dakejahl dakejahl May 21, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Worth a comment that Descend here is the framework cascade RTL→Land→Descend kicking in (because local_altitude_invalid blocks AUTO_RTL/AUTO_LAND), not what checkModeFallback directly returns. Reads as surprising on a first pass otherwise.

time += 10_ms;
failsafe.update(time, state, false, false, failsafe_flags);
// checkModeFallback returns RTL from NAV_RCL_ACT. The framework then cascades RTL -> Land -> Descend
// because local altitude loss blocks both AUTO_RTL and AUTO_LAND.
EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
}
Loading