From 55f395a7e9d75bfea7b4d307400225a321e46347 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 4 Aug 2022 13:36:15 +0200 Subject: [PATCH] FlightTaskAuto: apply cruise speed from position triplet also when negative (#20006) Navigator sets the cruise_speed to -1 if the controller shouldn't listen to it and instead use the default speed (for MC: MPC_XY_CRUISE). This is for example for RTL the case, where we want to return at the default speed, independetly of what the mission speed before was. Signed-off-by: Silvan Fuhrer --- src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index f31bf55ea8..09f8acf2fc 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -342,12 +342,11 @@ bool FlightTaskAuto::_evaluateTriplets() const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed; if (PX4_ISFINITE(cruise_speed_from_triplet) - && (cruise_speed_from_triplet > 0.f) && (_sub_triplet_setpoint.get().current.timestamp > _time_last_cruise_speed_override)) { _mc_cruise_speed = cruise_speed_from_triplet; } - if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { + if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < FLT_EPSILON)) { // If no speed is planned use the default cruise speed as limit _mc_cruise_speed = _param_mpc_xy_cruise.get(); }