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

Commander framework: only warn and do not re-start RTL if already in RTL

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-09-26 09:24:22 +02:00
parent 73c4ff302e
commit 8fdf0931e5
2 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -369,3 +369,26 @@ TEST_F(FailsafeTest, defer)
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
ASSERT_FALSE(failsafe.failsafeDeferred());
}
TEST_F(FailsafeTest, skip_failsafe)
{
FailsafeTester failsafe(nullptr);
failsafe_flags_s failsafe_flags{};
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 5_s;
uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
// RC lost while in RTL -> stay in RTL and only warn
failsafe_flags.manual_control_signal_lost = true;
updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn);
}

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

@ -608,6 +608,15 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
}
}
// If already in RTL, do not go into RTL again (would cause a Hold delay first, then re-start RTL)
if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
if ((selected_action == Action::RTL || returned_state.delayed_action == Action::RTL)
&& modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
selected_action = Action::Warn;
returned_state.delayed_action = Action::None;
}
}
// If already precision landing, do not go into RTL or Land
if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) {
if ((selected_action == Action::RTL || selected_action == Action::Land ||