From 57a2ea0cd1bc98b4dbcc68670feacf048370bd84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jun 2017 13:07:18 +0200 Subject: [PATCH] Navigator: Do not change target position if user is only changing altitude This is important for multicopters for drift-free altitude changes and important for fixed wing to keep the current loiter center. --- src/modules/navigator/navigator_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 420efaa071..1adc4e70e1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -383,6 +383,7 @@ Navigator::task_main() if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); + struct position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; @@ -406,6 +407,12 @@ Navigator::task_main() rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; + } else if (curr->current.valid + && PX4_ISFINITE(curr->current.lat) + && PX4_ISFINITE(curr->current.lon)) { + rep->current.lat = curr->current.lat; + rep->current.lon = curr->current.lon; + } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon;