dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW Position Controller: handle IDLE waypoints in FW_POSCTRL_MODE_AUTO, also with NAN setpoints 

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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-09-21 14:51:20 +02:00 committed by Daniel Agar
parent 080e16f0a6
commit 2f5aa92596
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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) {