From 6c0539c243a13ed0cb2c4c508b4770bdff57add6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:55:55 +0200 Subject: [PATCH] FW position controller: Do handle idle mission items correctly --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b5861d0f16..90cf391536 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1085,7 +1085,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -1544,6 +1549,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :