airframes: set defaults for manual vertical velocity limits, where necessary

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -74,6 +74,7 @@ param set-default MPC_XY_VEL_MAX 5
 param set-default MPC_LAND_SPEED 1.2
 param set-default MPC_TILTMAX_LND 35
 param set-default MPC_Z_VEL_MAX_UP 1.5
+param set-default MPC_Z_VEL_MAN_UP 1.5
 param set-default MPC_TKO_RAMP_T 0.8
 param set-default MPC_TILTMAX_AIR 25
 param set-default MPC_TILTMAX_LND 25

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

@@ -60,6 +60,7 @@ param set-default MPC_THR_HOVER 0.45
 param set-default MPC_TILTMAX_AIR 25
 param set-default MPC_YAWRAUTO_MAX 40
 param set-default MPC_Z_VEL_MAX_UP 2
+param set-default MPC_Z_VEL_MAN_UP 2
 
 param set-default NAV_ACC_RAD 3
 

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -123,6 +123,7 @@ param set-default MPC_TILTMAX_AIR 30
 param set-default MPC_XY_P 1
 param set-default MPC_Z_P 2
 param set-default MPC_Z_VEL_MAX_DN 2
+param set-default MPC_Z_VEL_MAN_DN 2
 
 # gimbal configuration
 param set-default MNT_MODE_IN 0

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -117,6 +117,12 @@ void FlightModeManager::Run()
 
 		if (isAnyTaskActive()) {
 			generateTrajectorySetpoint(dt, vehicle_local_position);
+
+		} else {
+			// Publish empty constraints (constraints from previous flight tasks must be overridden)
+			vehicle_constraints_s vehicle_constraints = FlightTask::empty_constraints;
+			vehicle_constraints.timestamp = time_stamp_now;
+			_vehicle_constraints_pub.publish(vehicle_constraints);
 		}
 
 	}

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp

@@ -50,8 +50,8 @@ bool FlightTaskManualAccelerationSlow::update()
 	// Limits which can only slow down from the nominal configuration we initialize with here
 	// This is ensured by the executing classes
 	float velocity_horizontal = _param_mpc_vel_manual.get();
-	float velocity_up = _param_mpc_z_vel_max_up.get();
-	float velocity_down = _param_mpc_z_vel_max_dn.get();
+	float velocity_up = _param_mpc_z_v_man_up.get();
+	float velocity_down = _param_mpc_z_v_man_dn.get();
 	float yaw_rate = math::radians(_param_mpc_man_y_max.get());
 
 	// MAVLink commanded limits

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp

@@ -81,8 +81,6 @@ private:
 					(ParamFloat<px4::params::MC_SLOW_DEF_VVEL>) _param_mc_slow_def_vvel,
 					(ParamFloat<px4::params::MC_SLOW_DEF_YAWR>) _param_mc_slow_def_yawr,
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
-					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
-					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
 					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
 				       )
 };

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

@@ -72,6 +72,15 @@ bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoi
 	return ret;
 }
 
+void FlightTaskManualAltitude::_setDefaultConstraints()
+{
+	FlightTask::_setDefaultConstraints();
+
+	// Apply velocity limits for manual modes
+	_constraints.speed_up = _param_mpc_z_v_man_up.get();
+	_constraints.speed_down = _param_mpc_z_v_man_dn.get();
+}
+
 void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 {
 	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
@@ -92,8 +101,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 void FlightTaskManualAltitude::_scaleSticks()
 {
 	// Use sticks input with deadzone and exponential curve for vertical velocity
-	const float vel_max_up = fminf(_param_mpc_z_vel_max_up.get(), _velocity_constraint_up);
-	const float vel_max_down = fminf(_param_mpc_z_vel_max_dn.get(), _velocity_constraint_down);
+	const float vel_max_up = fminf(_param_mpc_z_v_man_up.get(), _velocity_constraint_up);
+	const float vel_max_down = fminf(_param_mpc_z_v_man_dn.get(), _velocity_constraint_down);
 	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? vel_max_down : vel_max_up;
 	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 }
@@ -234,10 +243,10 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 		// 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),
-								-_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
+								-_param_mpc_z_v_man_dn.get(), _param_mpc_z_v_man_up.get());
 
 		} else {
-			_constraints.speed_up = _param_mpc_z_vel_max_up.get();
+			_constraints.speed_up = _param_mpc_z_v_man_up.get();
 		}
 
 		// if distance to bottom exceeded maximum distance, slowly approach maximum distance
@@ -247,10 +256,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(_param_mpc_z_vel_max_dn.get(), 0.7f);
+			_constraints.speed_down = math::min(_param_mpc_z_v_man_dn.get(), 0.7f);
 
 		} else {
-			_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+			_constraints.speed_down = _param_mpc_z_v_man_dn.get();
 		}
 	}
 }

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

@@ -59,6 +59,7 @@ protected:
 	virtual void _updateSetpoints(); /**< updates all setpoints */
 	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
 	bool _checkTakeoff() override;
+	void _setDefaultConstraints() override;
 	void _updateConstraintsFromEstimator();
 
 	/**
@@ -83,6 +84,8 @@ protected:
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
 					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
+					(ParamFloat<px4::params::MPC_Z_V_MAN_UP>) _param_mpc_z_v_man_up, /**< maximum upwards velocity */
+					(ParamFloat<px4::params::MPC_Z_V_MAN_DN>) _param_mpc_z_v_man_dn, /**< maximum downwards velocity */
 					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
 					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below which to land with land speed */
 					(ParamFloat<px4::params::MPC_LAND_SPEED>)

src/modules/mc_pos_control/GotoControl/GotoControl.cpp

@@ -123,14 +123,6 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
 
 	trajectory_setpoint.timestamp = goto_setpoint.timestamp;
 	_trajectory_setpoint_pub.publish(trajectory_setpoint);
-
-	vehicle_constraints_s vehicle_constraints{
-		.timestamp = goto_setpoint.timestamp,
-		.speed_up = NAN,
-		.speed_down = NAN,
-		.want_takeoff = false
-	};
-	_vehicle_constraints_pub.publish(vehicle_constraints);
 }
 
 void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)

src/modules/mc_pos_control/GotoControl/GotoControl.hpp

@@ -51,7 +51,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/goto_setpoint.h>
 #include <uORB/topics/trajectory_setpoint.h>
-#include <uORB/topics/vehicle_constraints.h>
 
 class GotoControl
 {
@@ -117,7 +116,6 @@ private:
 
 	uORB::SubscriptionData<goto_setpoint_s> _goto_setpoint_sub{ORB_ID(goto_setpoint)};
 	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
-	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
 
 	PositionSmoothing _position_smoothing;
 	HeadingSmoothing _heading_smoothing;

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -225,26 +225,48 @@ void MulticopterPositionControl::parameters_update(bool force)
 					    "Manual sideways speed has been constrained by forward speed", _param_mpc_vel_manual.get());
 		}
 
+		if (_param_mpc_z_v_man_up.get() > _param_mpc_z_vel_max_up.get()) {
+			_param_mpc_z_v_man_up.set(_param_mpc_z_vel_max_up.get());
+			_param_mpc_z_v_man_up.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Manual ascent speed has been constrained by max speed\t");
+			/* EVENT
+			 * @description <param>MPC_Z_V_MAN_UP</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("mc_pos_ctrl_manual_up_vel_set"), events::Log::Warning,
+					    "Manual ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get());
+		}
+
+		if (_param_mpc_z_v_man_dn.get() > _param_mpc_z_vel_max_dn.get()) {
+			_param_mpc_z_v_man_dn.set(_param_mpc_z_vel_max_dn.get());
+			_param_mpc_z_v_man_dn.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Manual descent speed has been constrained by max speed\t");
+			/* EVENT
+			 * @description <param>MPC_Z_V_MAN_DN</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("mc_pos_ctrl_manual_down_vel_set"), events::Log::Warning,
+					    "Manual descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.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");
+			mavlink_log_critical(&_mavlink_log_pub, "Auto 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());
+					    "Auto 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");
+			mavlink_log_critical(&_mavlink_log_pub, "Auto 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());
+					    "Auto 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() ||

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -140,8 +140,10 @@ private:
 		(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_V_MAN_UP>)   _param_mpc_z_v_man_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_V_MAN_DN>)   _param_mpc_z_v_man_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/multicopter_altitude_mode_params.c

@@ -55,6 +55,34 @@ PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.f);
  */
 PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.f);
 
+/**
+ * Maximum upwards velocity in Altitude/Position mode
+ *
+ * Should be less than MPC_Z_VEL_MAX_UP.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 8
+ * @increment 0.1
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_V_MAN_UP, 3.f);
+
+/**
+ * Maximum downwards velocity in Altitude/Position mode
+ *
+ * Should be less than MPC_Z_VEL_MAX_DN.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 4
+ * @increment 0.1
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_V_MAN_DN, 1.5f);
+
 /**
  * Manual yaw rate input filter time constant
  *