From 5590ab3caa81b74a97ab0ef0ae1182da3261e7e1 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 21 Sep 2023 17:27:54 +0200 Subject: [PATCH] FW Position Controller: handle IDLE waypoints in FW_POSCTRL_MODE_AUTO, also with NAN setpoints (#22114) A setpoint of type IDLE can be published by Navigator without a valid position, and should be handled in the auto function in FW_POSCTRL_MODE_AUTO. If it wouldn't be handled there the controller would switch to mode FW_POSCTRL_MODE_OTHER, in which case no attitude setpoint is published and the lower controllers would be stuck with the last published value (incl. thrust). Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 86c6d5bc20..a7f5bb5eab 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -723,7 +723,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _skipping_takeoff_detection = false; if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) || - _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) { + _control_mode.flag_control_offboard_enabled) && (_position_setpoint_current_valid + || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + + // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE. + // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {