From 0987bb2e8e5e8d1036b21bacc7835efec0e032bd Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 3 Jun 2022 09:34:57 -0700 Subject: [PATCH] Fix bug with yaw only reposition Fix formatting --- src/modules/navigator/navigator_main.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e3ff9406e0..c03a82198d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -303,12 +303,18 @@ void Navigator::run() rep->current.alt = get_global_position()->alt; } - } else if (PX4_ISFINITE(cmd.param7)) { - // Received only a request to change altitude, thus we keep the setpoint + } else if (PX4_ISFINITE(cmd.param7) || PX4_ISFINITE(cmd.param4)) { + // Position is not changing, thus we keep the setpoint rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat; rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon; - rep->current.alt = cmd.param7; - only_alt_change_requested = true; + + if (PX4_ISFINITE(cmd.param7)) { + rep->current.alt = cmd.param7; + only_alt_change_requested = true; + + } else { + rep->current.alt = get_global_position()->alt; + } } else { // All three set to NaN - pause vehicle