From 37015c3dbe3820dd36832a0f8f490e4eda7877bd Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Mon, 21 Aug 2023 16:51:12 +0200 Subject: [PATCH] FixedwingPositionControl: track single point when no prev point exists for waypoint following make sure correct local position setpoint output is logged --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index be267da592..cebeb8207b 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1036,8 +1036,11 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co prev_wp(1) = pos_sp_prev.lon; } else { - // No valid previous waypoint, go along the line between aircraft and current waypoint - prev_wp = curr_pos; + // No valid previous waypoint, fly directly to current waypoint + // NOTE: this is a bad interface - navigateWaypoints() takes two waypoints on top of each other as an + // indication it should fly directly to that waypoint. Better would be e.g. an alternative overload with + // singular setpoint input. (just an idea for future refactoring/refinement) + prev_wp = curr_wp; } float tecs_fw_thr_min;