From 7ec8dd9d23cd6112641b5cf2862a5288b3100ea6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 9 Nov 2021 13:13:45 +0100 Subject: [PATCH] vehicle_constraints: remove deprecated speed_xy constraint --- msg/vehicle_constraints.msg | 1 - .../tasks/Auto/FlightTaskAuto.cpp | 12 +----------- .../tasks/Auto/FlightTaskAuto.hpp | 1 - .../tasks/FlightTask/FlightTask.cpp | 3 +-- .../FlightTaskManualPosition.cpp | 19 ++++++------------- .../FlightTaskManualPosition.hpp | 1 - .../FlightTaskManualPositionSmoothVel.cpp | 2 +- .../MulticopterPositionControl.cpp | 7 ++----- .../MulticopterPositionControl.hpp | 1 - 9 files changed, 11 insertions(+), 36 deletions(-) diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg index 0e624c0370..aa3a491b12 100644 --- a/msg/vehicle_constraints.msg +++ b/msg/vehicle_constraints.msg @@ -3,7 +3,6 @@ uint64 timestamp # time since system start (microseconds) -float32 speed_xy # in meters/sec float32 speed_up # in meters/sec float32 speed_down # in meters/sec diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index a2548be0ce..032310220e 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -160,7 +160,7 @@ bool FlightTaskAuto::_evaluateTriplets() if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { // If no speed is planned use the default cruise speed as limit - _mc_cruise_speed = _constraints.speed_xy; + _mc_cruise_speed = _param_mpc_xy_cruise.get(); } // Ensure planned cruise speed is below the maximum such that the smooth trajectory doesn't get capped @@ -412,16 +412,6 @@ bool FlightTaskAuto::_evaluateGlobalReference() return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon); } -void FlightTaskAuto::_setDefaultConstraints() -{ - FlightTask::_setDefaultConstraints(); - - // only adjust limits if the new limit is lower - if (_constraints.speed_xy >= _param_mpc_xy_cruise.get()) { - _constraints.speed_xy = _param_mpc_xy_cruise.get(); - } -} - Vector2f FlightTaskAuto::_getTargetVelocityXY() { // guard against any bad velocity values diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 69180e8c59..ee4a7c013b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -93,7 +93,6 @@ public: void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} protected: - void _setDefaultConstraints() override; matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 3aa8cf899a..7d7a4a9ad5 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout; // First index of empty_setpoint corresponds to time-stamp and requires a finite number. const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}}; -const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, false, {}}; +const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint) @@ -201,7 +201,6 @@ void FlightTask::_evaluateDistanceToGround() void FlightTask::_setDefaultConstraints() { - _constraints.speed_xy = _param_mpc_xy_vel_max.get(); _constraints.speed_up = _param_mpc_z_vel_max_up.get(); _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); _constraints.want_takeoff = false; diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 5ef59be4bb..27ba94165b 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -61,15 +61,9 @@ bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s // all requirements from altitude-mode still have to hold bool ret = FlightTaskManualAltitude::activate(last_setpoint); - // set task specific constraint - if (_constraints.speed_xy >= _param_mpc_vel_manual.get()) { - _constraints.speed_xy = _param_mpc_vel_manual.get(); - } - _position_setpoint(0) = _position(0); _position_setpoint(1) = _position(1); _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; - _velocity_scale = _constraints.speed_xy; // for position-controlled mode, we need a valid position and velocity state // in NE-direction @@ -92,25 +86,24 @@ void FlightTaskManualPosition::_scaleSticks() const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max; + float velocity_scale = _param_mpc_vel_manual.get(); + if (PX4_ISFINITE(max_speed_from_estimator)) { // use the minimum of the estimator and user specified limit - _velocity_scale = fminf(_constraints.speed_xy, max_speed_from_estimator); + velocity_scale = fminf(velocity_scale, max_speed_from_estimator); // Allow for a minimum of 0.3 m/s for repositioning - _velocity_scale = fmaxf(_velocity_scale, 0.3f); - - } else { - _velocity_scale = _constraints.speed_xy; + velocity_scale = fmaxf(velocity_scale, 0.3f); } // scale velocity to its maximum limits - Vector2f vel_sp_xy = stick_xy * _velocity_scale; + Vector2f vel_sp_xy = stick_xy * velocity_scale; /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); // collision prevention if (_collision_prevention.is_active()) { - _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, _position.xy(), _velocity.xy()); + _collision_prevention.modifySetpoint(vel_sp_xy, velocity_scale, _position.xy(), _velocity.xy()); } _velocity_setpoint.xy() = vel_sp_xy; diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index b0db7c3387..3b93e1e71f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -69,7 +69,6 @@ protected: (ParamFloat) _param_mpc_hold_max_xy ) private: - float _velocity_scale{0.0f}; //scales the stick input to velocity uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ WeatherVane *_weathervane_yaw_handler = diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 5d61f74a14..6ebc0f2316 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -120,7 +120,7 @@ void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() { _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); - _smoothing_xy.setMaxVel(_constraints.speed_xy); + _smoothing_xy.setMaxVel(_param_mpc_vel_manual.get()); } void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 26df37d49d..0622f22250 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -378,7 +378,6 @@ void MulticopterPositionControl::Run() } // override with defaults - _vehicle_constraints.speed_xy = _param_mpc_xy_vel_max.get(); _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); _vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get(); } @@ -420,8 +419,6 @@ void MulticopterPositionControl::Run() PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get()); const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down : _param_mpc_z_vel_max_dn.get(); - const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy : - _param_mpc_xy_vel_max.get(); // Allow ramping from zero thrust on takeoff const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f; @@ -429,7 +426,7 @@ void MulticopterPositionControl::Run() _control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get()); _control.setVelocityLimits( - math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()), + _param_mpc_xy_vel_max.get(), math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit math::max(speed_down, 0.f)); @@ -466,7 +463,7 @@ void MulticopterPositionControl::Run() failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe); // reset constraints - _vehicle_constraints = {0, NAN, NAN, NAN, false, {}}; + _vehicle_constraints = {0, NAN, NAN, false, {}}; _control.setInputSetpoint(failsafe_setpoint); _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 424c8deb3a..59101622cb 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -113,7 +113,6 @@ private: vehicle_constraints_s _vehicle_constraints { .timestamp = 0, - .speed_xy = NAN, .speed_up = NAN, .speed_down = NAN, .want_takeoff = false,