Skip to content

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

Open
AkaiEurus wants to merge 2 commits into
PX4:mainfrom
AkaiEurus:fix-failsafe-manual-fallback-rc-loss
Open

fix(commander): failsafe prevent manual fallback without RC#27408
AkaiEurus wants to merge 2 commits into
PX4:mainfrom
AkaiEurus:fix-failsafe-manual-fallback-rc-loss

Conversation

@AkaiEurus
Copy link
Copy Markdown
Contributor

Solved Problem

Fixes manual mode fallback without RC in additional failsafe paths.

When manual control is unavailable but RC loss is ignored, losing position in Position mode could fall back to Altitude mode, and losing altitude in Altitude mode could fall back to Stabilized mode. Both modes require manual control.

Solution

Apply the same RC availability check used for Offboard fallback to the PosCtl/PositionSlow -> AltCtl and AltCtl -> Stabilized fallback paths. If manual control is lost, use the configured NAV_RCL_ACT action instead.

Changelog Entry

For release notes:

Bugfix: Prevent failsafe fallback into manual modes without manual control

Test coverage

before:
https://github.com/user-attachments/assets/c3d8fad6-4f3b-432f-a2e1-3246c3685a9e

after:
https://github.com/user-attachments/assets/377b0993-2642-458a-a9b1-125453323f25

@github-actions github-actions Bot added kind:bug Something is broken or behaving incorrectly. kind:test Adds or improves tests. risk:safety-critical May affect arming, failsafe, control, navigation, or flight safety. scope:commander Arming, modes, failsafe, health checks, or vehicle state. scope:testing Unit, integration, fuzzing, or test data. labels May 20, 2026
Comment thread src/modules/commander/failsafe/failsafe_test.cpp
Comment thread src/modules/commander/failsafe/failsafe_test.cpp
@dakejahl dakejahl requested a review from MaEtUgR May 20, 2026 21:52
Copy link
Copy Markdown
Contributor

@dakejahl dakejahl left a comment

Choose a reason for hiding this comment

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

Fix is correct — modeCanRun() deliberately excludes mode_req_manual_control (framework.cpp:704), so the existing cascade leaves the vehicle in ALTCTL/STAB when RC is gone. Aligning with the offboard pattern is the right call. A few inline notes on consistency and test coverage.

if (status_flags.manual_control_signal_lost) {
ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get());
action = rc_loss_action.action;
return action;
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

The offboard branch above (line ~735) runs the same NAV_RCL_ACT lookup but doesn't return — it falls through to the rest of the function. Worth aligning so all three sites behave the same.

Concretely, with the early return, NAV_RCL_ACT=0 (Disabled, accepted by gcs_connection_loss_failsafe_mode::Disabled) returns Action::None and skips the bottom-of-function modeCanRun catch. Without the return, the cascade would continue. Either choice is defensible — just make them consistent.

if (status_flags.manual_control_signal_lost) {
ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get());
action = rc_loss_action.action;
return action;
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Third copy of the same pattern now (offboard + the two new blocks). Consider extracting:

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

…or moving the RC check to a single post-cascade step. Easier to keep them in sync next time someone adds a fallback path.

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

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.

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

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.

Use NAV_RCL_ACT from a single post-cascade check for manual fallback modes,
and add coverage for non-default NAV_RCL_ACT behavior.
Comment thread src/modules/commander/failsafe/failsafe_test.cpp
Comment thread src/modules/commander/failsafe/failsafe_test.cpp
Comment thread src/modules/commander/failsafe/failsafe_test.cpp
@AkaiEurus
Copy link
Copy Markdown
Contributor Author

Fix is correct — modeCanRun() deliberately excludes mode_req_manual_control (framework.cpp:704), so the existing cascade leaves the vehicle in ALTCTL/STAB when RC is gone. Aligning with the offboard pattern is the right call. A few inline notes on consistency and test coverage.

Thanks for the review. I updated the fallback handling to use a single post-cascade NAV_RCL_ACT override for manual-control fallback actions, so the offboard, PosCtl -> AltCtl, and AltCtl -> Stabilized paths stay consistent and no longer have separate early-return behavior.

I also added a sibling test with a non-default NAV_RCL_ACT value to verify the parameter linkage, and added a comment explaining why the AltCtl case ends up as Descend after the framework cascade.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

kind:bug Something is broken or behaving incorrectly. kind:test Adds or improves tests. risk:safety-critical May affect arming, failsafe, control, navigation, or flight safety. scope:commander Arming, modes, failsafe, health checks, or vehicle state. scope:testing Unit, integration, fuzzing, or test data.

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants