AutoSmoothVel: adjust controller constraints in emergency braking

When engaging auto mode at high vertical speed, we don't want to cut the velocity
trajectory setpoint in order to create a smooth deceleration.

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -435,6 +435,11 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
 		_trajectory[2].setMaxAccel(9.81f);
 		_trajectory[2].setMaxJerk(9.81f);
 
+		// If the current velocity is beyond the usual constraints, tell
+		// the controller to exceptionally increase its saturations to avoid
+		// cutting out the feedforward
+		_constraints.speed_down = math::max(fabsf(_trajectory[2].getCurrentVelocity()), _param_mpc_z_vel_max_dn.get());
+
 	} else if (_velocity_setpoint(2) < 0.f) { // up
 		float z_accel_constraint = _param_mpc_acc_up_max.get();
 		float z_vel_constraint = _param_mpc_z_vel_max_up.get();

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -429,7 +429,7 @@ void MulticopterPositionControl::Run()
 			_control.setVelocityLimits(
 				math::constrain(speed_horizontal, 0.f, _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::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get()));
+				math::max(speed_down, 0.f));
 
 			_control.setInputSetpoint(_setpoint);