diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index 8efc5c0201..d8c1bdfd28 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -93,6 +93,14 @@ bool FlightTaskAuto::_evaluateTriplets() return false; } + /* Always update cruise speed since that can change without waypoint changes */ + _mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed; + + if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { + /* Use default */ + _mc_cruise_speed = _mc_cruise_default.get(); + } + /* Get target waypoint. */ matrix::Vector3f target; map_projection_project(&_reference_position, @@ -133,13 +141,6 @@ bool FlightTaskAuto::_evaluateTriplets() } - _mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed; - - if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { - /* Use default */ - _mc_cruise_speed = _mc_cruise_default.get(); - } - _type = (WaypointType)_sub_triplet_setpoint->get().current.type; _prev_prev_wp = _prev_wp; // previous -1 is set to previsou