From 48f2b42e1208e995ce28193983346d3497b66323 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 26 Jan 2023 17:11:34 +0100 Subject: [PATCH] FWPositionController: remove factor of 2 for switching to LOITER if altitude is not reached Instead check if system has previously switched into LOITER to acheive the current WP of type POSITION, and in that case stay in LOITER until altitude is reached. Signed-off-by: Silvan Fuhrer --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ac2b97ffef00..29c456cc3555 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1025,11 +1025,14 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp } if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - // POSITION: achieve position setpoint altitude via loiter - // close to waypoint, but altitude error greater than twice acceptance + // Achieve position setpoint altitude via loiter when laterally close to WP. + // Detect if system has switchted into a Loiter before (check _position_sp_type), and in that + // case remove the dist_xy check (not switch out of Loiter until altitude is reached). if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f) && (dist_z > _param_nav_fw_alt_rad.get()) - && (dist_xy < 2.f * math::max(acc_rad, loiter_radius_abs))) { + && (dist_xy < math::max(acc_rad, loiter_radius_abs) + || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { + // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; }