From dbc7d805f27692eb2ea918c7fb81b90e2ce46d18 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Jul 2017 13:23:12 +0200 Subject: [PATCH] Navigator: Fix pause mode to not continue to next waypoint if already set --- src/modules/navigator/navigator_main.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1adc4e70e1..0c8dc06472 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -403,25 +403,32 @@ Navigator::task_main() rep->current.yaw = NAN; } + // Position change with optional altitude change if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { 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 + if (PX4_ISFINITE(cmd.param7)) { + rep->current.alt = cmd.param7; + + } else { + rep->current.alt = get_global_position()->alt; + } + + // Altitude without position change + + } else if (PX4_ISFINITE(cmd.param7) && 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; + rep->current.alt = cmd.param7; + + // All three set to NaN - hold in current position } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; - } - - if (PX4_ISFINITE(cmd.param7)) { - rep->current.alt = cmd.param7; - - } else { rep->current.alt = get_global_position()->alt; }