diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index f35a0b8fe1..78af0ade60 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -81,7 +81,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; _updateTrajConstraints(); _is_emergency_braking_active = false; - _commanded_speed_ts = 0; + _time_last_cruise_speed_override = 0; return ret; } @@ -226,7 +226,7 @@ bool FlightTaskAuto::update() void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s) { _mc_cruise_speed = cruise_speed_m_s; - _commanded_speed_ts = hrt_absolute_time(); + _time_last_cruise_speed_override = hrt_absolute_time(); } void FlightTaskAuto::_prepareLandSetpoints() @@ -354,8 +354,9 @@ bool FlightTaskAuto::_evaluateTriplets() // Prioritize cruise speed from the triplet when it's valid and more recent than the previously commanded cruise speed 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 - && _sub_triplet_setpoint.get().current.timestamp > _commanded_speed_ts) { + 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; } diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index cac7cefc31..277ff1638c 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -202,7 +202,7 @@ private: _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/ matrix::Vector3f _closest_pt; /**< closest point to the vehicle position on the line previous - target */ - hrt_abstime _commanded_speed_ts{0}; + hrt_abstime _time_last_cruise_speed_override{0}; ///< timestamp the cruise speed was last time overriden using DO_CHANGE_SPEED MapProjection _reference_position{}; /**< Class used to project lat/lon setpoint into local frame. */ float _reference_altitude{NAN}; /**< Altitude relative to ground. */