From de4b139540e81238dfdf5fdfbd74f7cce51ad79d Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 7 Feb 2023 14:53:43 +0100 Subject: [PATCH] FwPosControl: Update behavior of navigating to a waypoint when the previous waypoint is not valid. Go along the line of the current aircraft position to the desired waypoint. --- .../fw_path_navigation/FixedwingPositionControl.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 34ced43c84..713ce21e8d 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -1026,10 +1026,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co prev_wp(1) = pos_sp_prev.lon; } else { - // No valid previous waypoint, go for the current wp. - // This is automatically handled by the NPFG libraries. - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; + // No valid previous waypoint, go along the line between aircraft and current waypoint + prev_wp = curr_pos; } float tecs_fw_thr_min; @@ -1186,10 +1184,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons prev_wp(1) = pos_sp_prev.lon; } else { - // No valid previous waypoint, go for the current wp. - // This is automatically handled by the NPFG libraries. - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; + // No valid previous waypoint, go along the line between aircraft and current waypoint + prev_wp = curr_pos; } float airspeed_sp = -1.f;