From 38e635e386594e4e2cba9b0848dc63a14a001001 Mon Sep 17 00:00:00 2001 From: daniele Date: Mon, 30 Mar 2020 18:54:05 +0200 Subject: [PATCH] navigator_main: allow altitude change when the current lat and lon are still nan This is a corner case that happens when the user wants to command an altitude change just after a takeoff. Takeoff doesn't set the current lat and lon therefore the user had to change altitude twice to get it working. The first time to set the current lat and lon and the second time to actually change altitude. --- src/modules/navigator/navigator_main.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 01f4ac37bb..f858db125a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -300,13 +300,19 @@ Navigator::run() rep->current.alt = get_global_position()->alt; } - } else if (PX4_ISFINITE(cmd.param7) && curr->current.valid - && PX4_ISFINITE(curr->current.lat) - && PX4_ISFINITE(curr->current.lon)) { + } else if (PX4_ISFINITE(cmd.param7)) { // Altitude without position change - rep->current.lat = curr->current.lat; - rep->current.lon = curr->current.lon; + // This condition is necessary for altitude changes just after takeoff where lat and lon are still nan + 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; + } + rep->current.alt = cmd.param7; } else {