From afa085da7f934674f593c9980746c318e309bb93 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 21 Jul 2021 12:36:06 +0200 Subject: [PATCH] geofence: fix altitude change fence check When asking for an altitude change only, the current position needs to be used, otherwise the geofence check is be skipped --- src/modules/navigator/navigator_main.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4354272590..33b191cb24 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -257,8 +257,16 @@ void Navigator::run() bool reposition_valid = true; vehicle_global_position_s position_setpoint{}; - position_setpoint.lat = cmd.param5; - position_setpoint.lon = cmd.param6; + + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + position_setpoint.lat = cmd.param5; + position_setpoint.lon = cmd.param6; + + } else { + position_setpoint.lat = get_global_position()->lat; + position_setpoint.lon = get_global_position()->lon; + } + position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; if (have_geofence_position_data) {