vehicle_constraints: remove deprecated speed_xy constraint

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
 

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

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 */

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;

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);
+		velocity_scale = fmaxf(velocity_scale, 0.3f);
-
-	} else {
-		_velocity_scale = _constraints.speed_xy;
 	}
 
 	// 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;

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -69,7 +69,6 @@ protected:
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _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 =

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()

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());

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,