From ef774578e1c8e7e5f3a73e504918e73d32ed1f00 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 17 Mar 2021 13:47:38 +0100 Subject: [PATCH] FlightTaskOrbit: NACK command if coordinates could not get applied --- .../tasks/Orbit/FlightTaskOrbit.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 2ea61417c3..d643a67724 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -77,16 +77,24 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) _initial_heading = _yaw; // commanded center coordinates - if (map_projection_initialized(&_global_local_proj_ref)) { - if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) { + if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) { + if (map_projection_initialized(&_global_local_proj_ref)) { map_projection_project(&_global_local_proj_ref, command.param5, command.param6, &_center(0), &_center(1)); - } - // commanded altitude - if (PX4_ISFINITE(command.param7)) { + } else { + ret = false; + } + } + + // commanded altitude + if (PX4_ISFINITE(command.param7)) { + if (map_projection_initialized(&_global_local_proj_ref)) { _position_setpoint(2) = _global_local_alt0 - command.param7; + + } else { + ret = false; } }