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_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_down);
-		_constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_up.get());
+		_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();
+		const float z_accel_constraint = _param_mpc_acc_up_max.get();
-		float z_vel_constraint = _param_mpc_z_vel_max_up.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());
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
-		_altitude_velocity_smoothing.setMaxVel(_param_mpc_acc_down_max.get());
+		_altitude_velocity_smoothing.setMaxAccel(_param_mpc_acc_down_max.get());
-		_altitude_velocity_smoothing.setMaxAccel(_param_mpc_z_vel_max_dn.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
 	)
 };

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -127,7 +127,9 @@ int MulticopterPositionControl::parameters_update(bool force)
 
 		if (_param_mpc_z_vel_all.get() >= 0.f) {
 			float z_vel = _param_mpc_z_vel_all.get();
+			num_changed += _param_mpc_z_v_auto_up.commit_no_notification(z_vel);
 			num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel);
+			num_changed += _param_mpc_z_v_auto_dn.commit_no_notification(z_vel * 0.75f);
 			num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f);
 			num_changed += _param_mpc_tko_speed.commit_no_notification(z_vel * 0.6f);
 			num_changed += _param_mpc_land_speed.commit_no_notification(z_vel * 0.5f);
@@ -189,6 +191,28 @@ int MulticopterPositionControl::parameters_update(bool force)
 					    "Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get());
 		}
 
+		if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) {
+			_param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get());
+			_param_mpc_z_v_auto_up.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t");
+			/* EVENT
+			 * @description <param>MPC_Z_V_AUTO_UP</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("mc_pos_ctrl_up_vel_set"), events::Log::Warning,
+					    "Ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get());
+		}
+
+		if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) {
+			_param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get());
+			_param_mpc_z_v_auto_dn.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t");
+			/* EVENT
+			 * @description <param>MPC_Z_V_AUTO_DN</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("mc_pos_ctrl_down_vel_set"), events::Log::Warning,
+					    "Descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get());
+		}
+
 		if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
 		    _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
 			_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -137,7 +137,9 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>)  _param_mpc_z_vel_i_acc,
 		(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>)  _param_mpc_z_vel_d_acc,
 		(ParamFloat<px4::params::MPC_XY_VEL_MAX>)   _param_mpc_xy_vel_max,
+		(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>)  _param_mpc_z_v_auto_up,
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
+		(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>)  _param_mpc_z_v_auto_dn,
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
 		(ParamFloat<px4::params::MPC_TILTMAX_AIR>)  _param_mpc_tiltmax_air,
 		(ParamFloat<px4::params::MPC_THR_HOVER>)    _param_mpc_thr_hover,

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -200,29 +200,64 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f);
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f);
 
 /**
- * Maximum vertical ascent velocity
+ * Automatic ascent velocity
  *
- * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
+ * Ascent velocity in auto modes.
+ * For manual modes and offboard, see MPC_Z_VEL_MAX_UP
  *
  * @unit m/s
  * @min 0.5
  * @max 8.0
+ * @increment 0.1
  * @decimal 1
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f);
 
 /**
- * Maximum vertical descent velocity
+ * Maximum ascent velocity
  *
- * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
+ * Ascent velocity in manual modes and offboard.
+ * For auto modes, see MPC_Z_V_AUTO_UP
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 8.0
+ * @increment 0.1
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f);
+
+/**
+ * Automatic descent velocity
+ *
+ * Descent velocity in auto modes.
+ * For manual modes and offboard, see MPC_Z_VEL_MAX_DN
  *
  * @unit m/s
  * @min 0.5
  * @max 4.0
+ * @increment 0.1
+ * @decimal 1
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.f);
+
+/**
+ * Maximum descent velocity
+ *
+ * Descent velocity in manual modes and offboard.
+ * For auto modes, see MPC_Z_V_AUTO_DN
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 4.0
+ * @increment 0.1
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.f);
 
 /**
  * Proportional gain for horizontal position error
@@ -664,7 +699,7 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f);
  * Altitude for 1. step of slow landing (descend)
  *
  * Below this altitude descending velocity gets limited to a value
- * between "MPC_Z_VEL_MAX_DN" and "MPC_LAND_SPEED"
+ * between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED"
  * Value needs to be higher than "MPC_LAND_ALT2"
  *
  * @unit m

src/modules/navigator/rtl.cpp

@@ -57,8 +57,8 @@ RTL::RTL(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
 {
-	_param_mpc_z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP");
+	_param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP");
-	_param_mpc_z_vel_max_down = param_find("MPC_Z_VEL_MAX_DN");
+	_param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN");
 	_param_mpc_land_speed = param_find("MPC_LAND_SPEED");
 	_param_fw_climb_rate = param_find("FW_T_CLMB_R_SP");
 	_param_fw_sink_rate = param_find("FW_T_SINK_R_SP");
@@ -886,7 +886,7 @@ float RTL::getClimbRate()
 	float ret = 1e6f;
 
 	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-		if (_param_mpc_z_vel_max_up == PARAM_INVALID || param_get(_param_mpc_z_vel_max_up, &ret) != PX4_OK) {
+		if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) {
 			ret = 1e6f;
 		}
 
@@ -905,7 +905,7 @@ float RTL::getDescendRate()
 	float ret = 1e6f;
 
 	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-		if (_param_mpc_z_vel_max_down == PARAM_INVALID || param_get(_param_mpc_z_vel_max_down, &ret) != PX4_OK) {
+		if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) {
 			ret = 1e6f;
 		}
 

src/modules/navigator/rtl.h

@@ -177,8 +177,8 @@ private:
 		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin
 	)
 
-	param_t		_param_mpc_z_vel_max_up{PARAM_INVALID};
+	param_t		_param_mpc_z_v_auto_up{PARAM_INVALID};
-	param_t		_param_mpc_z_vel_max_down{PARAM_INVALID};
+	param_t		_param_mpc_z_v_auto_dn{PARAM_INVALID};
 	param_t		_param_mpc_land_speed{PARAM_INVALID};
 	param_t		_param_fw_climb_rate{PARAM_INVALID};
 	param_t		_param_fw_sink_rate{PARAM_INVALID};