From 0a40034159e38eded3eac6541ca5c417cf6013d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jun 2016 16:26:05 +0200 Subject: [PATCH] Takeoff: Fix coordinate scaling (#4947) --- src/modules/navigator/navigator_main.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 01e168335e..163238da75 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -436,8 +436,9 @@ Navigator::task_main() } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { - rep->current.lat = cmd.param5 / (double)1e7; - rep->current.lon = cmd.param6 / (double)1e7; + 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 { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; @@ -465,8 +466,16 @@ Navigator::task_main() rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.yaw = cmd.param4; - rep->current.lat = cmd.param5 / (double)1e7; - rep->current.lon = cmd.param6 / (double)1e7; + + 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 one of them is non-finite, reset both + rep->current.lat = NAN; + rep->current.lon = NAN; + } + rep->current.alt = cmd.param7; rep->previous.valid = true;