Expanded Vehicleconstraints to xy for emergency braking

msg/VehicleConstraints.msg

@@ -5,5 +5,6 @@ uint64 timestamp # time since system start (microseconds)
 
 float32 speed_up # in meters/sec
 float32 speed_down # in meters/sec
+float32 speed_xy # in meters/sec
 
 bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -812,6 +812,12 @@ void FlightTaskAuto::_updateTrajConstraints()
 	_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
 	_position_smoothing.setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading
 
+	// Stretch the constraints of the velocity controller to leave some room for an additional
+	// correction required by the altitude/vertical position controller
+	_constraints.speed_down = 1.2f * _param_mpc_z_v_auto_dn.get();
+	_constraints.speed_up = 1.2f * _param_mpc_xy_vel_max.get();
+	_constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get();;
+
 	if (_is_emergency_braking_active) {
 		// When initializing with large velocity, allow 1g of
 		// acceleration in 1s on all axes for fast braking
@@ -823,6 +829,7 @@ void FlightTaskAuto::_updateTrajConstraints()
 		// cutting out the feedforward
 		_constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_down);
 		_constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_up);
+		_constraints.speed_xy = math::max(_position_smoothing.getCurrentVelocityXY().norm(), _constraints.speed_xy);
 
 	} else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
 		float z_accel_constraint = _param_mpc_acc_up_max.get();
@@ -849,11 +856,6 @@ void FlightTaskAuto::_updateTrajConstraints()
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
 	}
-
-	// Stretch the constraints of the velocity controller to leave some room for an additional
-	// correction required by the altitude/vertical position controller
-	_constraints.speed_down = math::max(_constraints.speed_down, 1.2f * _param_mpc_z_v_auto_dn.get());;
-	_constraints.speed_up = math::max(_constraints.speed_up, 1.2f * _param_mpc_z_v_auto_up.get());;
 }
 
 bool FlightTaskAuto::_highEnoughForLandingGear()

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -525,12 +525,12 @@ 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();
-
 			// Allow ramping from zero thrust on takeoff
 			const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
 			_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
 
-			float max_speed_xy = _param_mpc_xy_vel_max.get();
+			float max_speed_xy = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_xy :
+					     _param_mpc_xy_vel_max.get();
 
 			if (PX4_ISFINITE(vehicle_local_position.vxy_max)) {
 				max_speed_xy = math::min(max_speed_xy, vehicle_local_position.vxy_max);