From 0381e148225ab8ef2380090319be9ebd4ca5608a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 8 Aug 2024 13:51:00 +0200 Subject: [PATCH] FlightTaskOrbit: Avoid sending NAN altitude in status telemetry while it's changed by stick --- .../flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 18adf4d3eb..144c6c7ba1 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -140,9 +140,11 @@ bool FlightTaskOrbit::sendTelemetry() orbit_status.yaw_behaviour = _yaw_behaviour; if (_geo_projection.isInitialized()) { + // While chainging altitude by stick _position_setpoint(2) is not set (NAN) + float local_altitude = PX4_ISFINITE(_position_setpoint(2)) ? _position_setpoint(2) : _position(2); // local -> global _geo_projection.reproject(_center(0), _center(1), orbit_status.x, orbit_status.y); - orbit_status.z = _global_local_alt0 - _position_setpoint(2); + orbit_status.z = _global_local_alt0 - local_altitude; } else { return false; // don't send the message if the transformation failed