From 8c8120e2fc9e66e3962fe123881d57ff6ef94d05 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 27 Jun 2016 10:54:30 +0200 Subject: [PATCH] fw position controller: logic cleanup cleaned up logic when not to use pitch setpoint from TECS Signed-off-by: tumbili --- .../fw_pos_control_l1_main.cpp | 27 ++++++++++++------- 1 file changed, 18 insertions(+), 9 deletions(-) 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 8390266d9f..90879d9811 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 @@ -2065,15 +2065,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - /* During a takeoff waypoint while waiting for launch the pitch sp is set - * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - (_launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled())) || - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && - _land_noreturn_vertical)) - )) { + // decide when to use pitch setpoint from TECS because in some cases pitch + // setpoint is generated by other means + bool use_tecs_pitch = true; + + // auto runway takeoff + use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())); + + + // flaring during landing + use_tecs_pitch &= !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + _land_noreturn_vertical); + + // manual attitude control + use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER); + + if (use_tecs_pitch) { _att_sp.pitch_body = get_tecs_pitch(); }