fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

[1.17] Bugfix: Let user take over from a degraded failsafe (#26269)

Browse Source 
* astyle: remove max line length (#25717)

* failsafe unit test: add cases for 1 allow taking over from degraded failsafes 2 not cause immediate takeover when failsafe happens because of mode switch

The first test makes sure the user can take over when an RTL failsafe was triggered but degraded to a Land.

The second test rules out the easiest fix of removing the condition `_selected_action == selected_action` which causes the problem for test one but is there for a reason.

* commander/failsafe: fix user takeover not possible in fallback Land action when configured RTL is not possible

---------

Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
This commit is contained in:
Matthias Grob 2026-01-15 20:48:53 +01:00 committed by GitHub
parent 82532f9642
commit e841fe029a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 80 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/astyle/astylerc
View File

@ -15,4 +15,4 @@ ignore-exclude-errors-x
lineend=linux
exclude=EASTL
add-brackets
max-code-length=120
max-code-length=140

80
src/modules/commander/failsafe/failsafe_test.cpp
View File

@ -35,6 +35,7 @@
#include "framework.h"
#include <uORB/topics/vehicle_status.h>
#include "../ModeUtil/mode_requirements.hpp"
// to run: make tests TESTFILTER=failsafe_test
@ -50,16 +51,16 @@ protected:
void checkStateAndMode(const hrt_abstime &time_us, const State &state,
const failsafe_flags_s &status_flags) override
{
CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
CHECK_FAILSAFE(status_flags, manual_control_signal_lost, ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
CHECK_FAILSAFE(status_flags, gcs_connection_lost, Action::Descend);
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
CHECK_FAILSAFE(status_flags, mission_failure, Action::Descend);
}
CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
CHECK_FAILSAFE(status_flags, wind_limit_exceeded, ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::RemainingFlightTimeLow));
CHECK_FAILSAFE(status_flags, offboard_control_signal_lost, ActionOptions(Action::Hold));
_last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure
&& status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
@ -258,6 +259,77 @@ TEST_F(FailsafeTest, takeover_denied)
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
}
TEST_F(FailsafeTest, can_takeover_degraded_failsafe)
{
FailsafeTester failsafe(nullptr);
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_MANUAL;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 3847124342;
failsafe_flags_s failsafe_flags{};
mode_util::getModeRequirements(state.vehicle_type, failsafe_flags); // Load mode requirements to degrade without valid position estimate
bool user_intended_mode_updated = false;
uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
// Battery time low -> Hold for the delay
time += 10_ms;
failsafe_flags.battery_low_remaining_time = true;
updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
// Delay over -> RTL
time += 5_s;
failsafe_flags.battery_low_remaining_time = true;
updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
// Global position gets invalid -> Land
time += 10_ms;
failsafe_flags.global_position_invalid = true;
updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Land);
// User wants takeover -> Altitude mode + Warning
time += 10_ms;
user_intended_mode_updated = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn);
ASSERT_TRUE(failsafe.userTakeoverActive());
}
TEST_F(FailsafeTest, no_immediate_takeover_when_failsafe_on_mode_switch)
{
FailsafeTester failsafe(nullptr);
failsafe_flags_s failsafe_flags{};
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 3847124342;
bool user_intended_mode_updated = false;
uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
// Switch to offboard but no offboard signal -> No immediate user takeover flagged but rather Hold
time += 10_ms;
user_intended_mode_updated = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
failsafe_flags.offboard_control_signal_lost = true;
updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
ASSERT_FALSE(failsafe.userTakeoverActive());
}
TEST_F(FailsafeTest, defer)
{
FailsafeTester failsafe(nullptr);

6
src/modules/commander/failsafe/framework.cpp
View File

@ -499,9 +499,9 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly;
}
// User takeover is activated on user intented mode update (w/o action change, so takeover is not immediately
// requested when entering failsafe) or rc stick movements
bool want_user_takeover_mode_switch = user_intended_mode_updated && _selected_action == selected_action;
// User takeover interrupting a failsafe is triggered by a change of the user-intended mode
// (only if a failsafe action is already active otherwise there can be immediate takeover when entering a failsafe) or by stick movement
bool want_user_takeover_mode_switch = user_intended_mode_updated && (_selected_action > Action::Warn);
bool want_user_takeover = want_user_takeover_mode_switch || rc_sticks_takeover_request;
bool takeover_allowed =
(allow_user_takeover == UserTakeoverAllowed::Always && (_user_takeover_active || want_user_takeover))