mpc: add parameter for ascent/descent speed in auto modes

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -72,9 +72,6 @@ bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s
 
 	_updateConstraintsFromEstimator();
 
-	_max_speed_up = _constraints.speed_up;
-	_max_speed_down = _constraints.speed_down;
-
 	return ret;
 }
 
@@ -102,7 +99,8 @@ void FlightTaskManualAltitude::_scaleSticks()
 	_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
 
 	// Use sticks input with deadzone and exponential curve for vertical velocity
-	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
+	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() :
+				_param_mpc_z_vel_max_up.get();
 	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 }
 
@@ -249,12 +247,13 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 
 		// if there is a valid maximum distance to ground, linearly increase speed limit with distance
 		// below the maximum, preserving control loop vertical position error gain.
+		// TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller
 		if (PX4_ISFINITE(_max_distance_to_ground)) {
 			_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom),
-								-_max_speed_down, _max_speed_up);
+								-_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
 
 		} else {
-			_constraints.speed_up = _max_speed_up;
+			_constraints.speed_up = _param_mpc_z_vel_max_up.get();
 		}
 
 		// if distance to bottom exceeded maximum distance, slowly approach maximum distance
@@ -264,10 +263,10 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 			// set position setpoint to maximum distance to ground
 			_position_setpoint(2) = _position(2) + delta_distance_to_max;
 			// limit speed downwards to 0.7m/s
-			_constraints.speed_down = math::min(_max_speed_down, 0.7f);
+			_constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f);
 
 		} else {
-			_constraints.speed_down = _max_speed_down;
+			_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
 		}
 	}
 }

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -134,8 +134,6 @@ private:
 
 	float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
-	float _max_speed_up = 10.0f;
-	float _max_speed_down = 1.0f;
 	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
 	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */