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(); }