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

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

@@ -231,7 +231,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	// Slow down automatic descend close to ground
 	float land_speed = math::gradual(_dist_to_ground,
 					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-					 _param_mpc_land_speed.get(), _constraints.speed_down);
+					 _param_mpc_land_speed.get(), _param_mpc_z_v_auto_dn.get());
 
 	if (_type_previous != WaypointType::land) {
 		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
@@ -802,12 +802,12 @@ void FlightTaskAuto::_updateTrajConstraints()
 		// 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(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_dn.get());
-		_constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_up.get());
+		_constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_down);
+		_constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_up);
 
 	} else if (_unsmoothed_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();
+		float z_vel_constraint = _param_mpc_z_v_auto_up.get();
 
 		// The constraints are broken because they are used as hard limits by the position controller, so put this here
 		// until the constraints don't do things like cause controller integrators to saturate. Once the controller
@@ -828,7 +828,7 @@ void FlightTaskAuto::_updateTrajConstraints()
 
 	} else { // down
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
-		_position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get());
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
 	}
 }
 

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

@@ -176,6 +176,8 @@ protected:
 					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
 					(ParamFloat<px4::params::MPC_LAND_ALT2>)
 					_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
+					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
+					(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
 					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
 					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
 					_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp

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

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -248,19 +248,18 @@ void FlightTaskOrbit::_updateTrajectoryBoundaries()
 	_altitude_velocity_smoothing.setMaxJerk(max_jerk);
 
 	if (_unsmoothed_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();
+		const float z_accel_constraint = _param_mpc_acc_up_max.get();
 
-		_position_smoothing.setMaxVelocityZ(z_vel_constraint);
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get());
 		_position_smoothing.setMaxAccelerationZ(z_accel_constraint);
-		_altitude_velocity_smoothing.setMaxVel(z_vel_constraint);
+		_altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_up.get());
 		_altitude_velocity_smoothing.setMaxAccel(z_accel_constraint);
 
 	} else { // down
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
-		_position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get());
-		_altitude_velocity_smoothing.setMaxVel(_param_mpc_acc_down_max.get());
-		_altitude_velocity_smoothing.setMaxAccel(_param_mpc_z_vel_max_dn.get());
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
+		_altitude_velocity_smoothing.setMaxAccel(_param_mpc_acc_down_max.get());
+		_altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_dn.get());
 	}
 
 }

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -140,6 +140,8 @@ private:
 		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
 		(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
 		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
-		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
+		(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
+		(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn
 	)
 };