From dbd9b88beaa1d7c86f8d2d319fd619c7ca7b156b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 5 Nov 2021 17:36:30 +0100 Subject: [PATCH] FW position controller: only allow switching to AUTO_ALTITDUE or AUTO_CLIMBRATE if pos_sp_curr_valid Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b9eb352419..50fbc231e7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -642,7 +642,8 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) { _control_mode_current = FW_POSCTRL_MODE_AUTO; - } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled) { + } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled + && pos_sp_curr_valid) { // reset timer the first time we switch into this mode if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE && _control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {