From dd83ef1813f4a4ce50adea843b24cdbfc08706c5 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Tue, 9 Nov 2021 15:10:13 +0100 Subject: [PATCH] Fix fw position controller takeoff This was introduced by a rebase --- .../FixedwingPositionControl.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d526c606b6..f037da53ac 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -772,6 +772,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp); + _position_sp_type = position_sp_type; + switch (position_sp_type) { case position_setpoint_s::SETPOINT_TYPE_IDLE: _att_sp.thrust_body[0] = 0.0f; @@ -797,12 +799,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c } /* reset landing state */ - if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) { + if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ - if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -812,7 +814,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c } /* Copy thrust output for publication, handle special cases */ - if (_position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && // launchdetector only available in auto + if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && // launchdetector only available in auto _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) { @@ -821,12 +823,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - } else if (_position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _runway_takeoff.runwayTakeoffEnabled()) { _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust()); - } else if (_position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust_body[0] = 0.0f; @@ -846,11 +848,11 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c bool use_tecs_pitch = true; // auto runway takeoff - use_tecs_pitch &= !(_position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + use_tecs_pitch &= !(position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())); // flaring during landing - use_tecs_pitch &= !(_position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical); + use_tecs_pitch &= !(position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical); if (use_tecs_pitch) { _att_sp.pitch_body = get_tecs_pitch();