From c267cf71c3aa57fbbe544d02f3b25650f029384d Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 20 Oct 2022 18:22:40 +0200 Subject: [PATCH] FW Position Control: fix entering of no-position-estimate failsafes Affects the states AUTO_ALTITUDE and AUTO_CLIMBRATE. Those modes should only be entered if armed (as they are pure failsafe modes). Also allow though to enter them even if the position setpoint(s) are invalid, as they are not needed. Signed-off-by: Silvan Fuhrer --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 6531210bdd..3391f01e8e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -789,12 +789,16 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _control_mode_current = FW_POSCTRL_MODE_AUTO; } - } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled - && pos_sp_curr_valid) { + } else if (_control_mode.flag_control_auto_enabled + && _control_mode.flag_control_climb_rate_enabled + && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes + && !_control_mode.flag_control_position_enabled) { + + // failsafe modes engaged if position estimate is invalidated - // reset timer the first time we switch into this mode if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE && commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) { + // reset timer the first time we switch into this mode _time_in_fixed_bank_loiter = now; }