mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 18:47:21 +08:00
fix commander: do not trigger Hold (delay) when failsafe action is Warn
The new unit test failed before and passes with the fix.
This commit is contained in:
@@ -62,6 +62,9 @@ protected:
|
|||||||
CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::RemainingFlightTimeLow));
|
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));
|
CHECK_FAILSAFE(status_flags, offboard_control_signal_lost, ActionOptions(Action::Hold));
|
||||||
|
|
||||||
|
CHECK_FAILSAFE(status_flags, navigator_failure, ActionOptions(Action::Warn));
|
||||||
|
CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, ActionOptions(Action::None));
|
||||||
|
|
||||||
_last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure
|
_last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure
|
||||||
&& status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
|
&& status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
|
||||||
}
|
}
|
||||||
@@ -305,6 +308,36 @@ TEST_F(FailsafeTest, can_takeover_degraded_failsafe)
|
|||||||
ASSERT_TRUE(failsafe.userTakeoverActive());
|
ASSERT_TRUE(failsafe.userTakeoverActive());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST_F(FailsafeTest, no_delay_for_warn)
|
||||||
|
{
|
||||||
|
// Ensure there is no Hold/delay when the current action is Warn
|
||||||
|
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{};
|
||||||
|
bool user_intended_mode_updated = false;
|
||||||
|
|
||||||
|
uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags);
|
||||||
|
|
||||||
|
// Navigator failure -> Warn
|
||||||
|
time += 10_ms;
|
||||||
|
failsafe_flags.navigator_failure = 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::Warn);
|
||||||
|
|
||||||
|
// Imbalanced props -> Warn (no delay)
|
||||||
|
time += 5_s;
|
||||||
|
failsafe_flags.fd_imbalanced_prop = 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::Warn);
|
||||||
|
}
|
||||||
|
|
||||||
TEST_F(FailsafeTest, no_immediate_takeover_when_failsafe_on_mode_switch)
|
TEST_F(FailsafeTest, no_immediate_takeover_when_failsafe_on_mode_switch)
|
||||||
{
|
{
|
||||||
FailsafeTester failsafe(nullptr);
|
FailsafeTester failsafe(nullptr);
|
||||||
|
|||||||
@@ -488,6 +488,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||||||
|
|
||||||
// Check if we should enter delayed Hold
|
// Check if we should enter delayed Hold
|
||||||
const bool action_can_be_delayed = selected_action != Action::None &&
|
const bool action_can_be_delayed = selected_action != Action::None &&
|
||||||
|
selected_action != Action::Warn &&
|
||||||
selected_action != Action::Disarm &&
|
selected_action != Action::Disarm &&
|
||||||
selected_action != Action::Terminate &&
|
selected_action != Action::Terminate &&
|
||||||
selected_action != Action::Hold;
|
selected_action != Action::Hold;
|
||||||
|
|||||||
Reference in New Issue
Block a user