diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index d23ab4be2d..2e2d182066 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -59,9 +59,12 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr bool FlightTaskAuto::activate() { + bool ret = FlightTask::activate(); _prev_prev_wp = _prev_wp = _target = _next_wp = _position; _yaw_wp = _yaw; - return FlightTask::activate(); + _setDefaultConstraints(); + + return ret; } bool FlightTaskAuto::updateInitialize() @@ -98,9 +101,9 @@ bool FlightTaskAuto::_evaluateTriplets() // 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) || (_mc_cruise_speed > _limits.speed_NE_max)) { + if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _constraints.speed_xy)) { // use default limit - _mc_cruise_speed = _limits.speed_NE_max; + _mc_cruise_speed = _constraints.speed_xy; } // get target waypoint. @@ -186,13 +189,12 @@ void FlightTaskAuto::_evaluateVehicleGlobalPosition() } } -void FlightTaskAuto::_updateSetpointLimits() +void FlightTaskAuto::_setDefaultConstraints() { - FlightTask::_updateSetpointLimits(); + FlightTask::_setDefaultConstraints(); // only adjust limits if the new limit is lower - if (_limits.speed_NE_max >= MPC_XY_CRUISE.get()) { - _limits.speed_NE_max = MPC_XY_CRUISE.get(); + if (_constraints.speed_xy >= MPC_XY_CRUISE.get()) { + _constraints.speed_xy = MPC_XY_CRUISE.get(); } - } diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index 876dcc8bf5..89dcafd798 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -70,7 +70,8 @@ public: bool updateInitialize() override; protected: - void _updateSetpointLimits() override; + void _setDefaultConstraints() override; + float _getMaxCruiseSpeed() {return MPC_XY_CRUISE.get();} /**< getter for default cruise speed */ matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */