PositionControl: replace constraint structure with new constraint message structure

src/modules/mc_pos_control/PositionControl.cpp

@@ -163,11 +163,11 @@ void PositionControl::_positionController()
 	// Constrain horizontal velocity by prioritizing the velocity component along the
 	// the desired position setpoint over the feed-forward term.
 	Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)),
-			     Vector2f(&(_vel_sp - vel_sp_position)(0)), MPC_XY_VEL_MAX.get());
+			     Vector2f(&(_vel_sp - vel_sp_position)(0)), _constraints.speed_xy);
 	_vel_sp(0) = vel_sp_xy(0);
 	_vel_sp(1) = vel_sp_xy(1);
 	// Constrain velocity in z-direction.
-	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.vel_max_z_up, MPC_Z_VEL_MAX_DN.get());
+	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
 }
 
 void PositionControl::_velocityController(const float &dt)
@@ -222,7 +222,7 @@ void PositionControl::_velocityController(const float &dt)
 	if (fabsf(_thr_sp(0)) + fabsf(_thr_sp(1))  > FLT_EPSILON) {
 		// Thrust set-point in NE-direction is already provided. Only
 		// scaling by the maximum tilt is required.
-		float thr_xy_max = fabsf(_thr_sp(2)) * tanf(MPC_MAN_TILT_MAX_rad.get());
+		float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
 		_thr_sp(0) *= thr_xy_max;
 		_thr_sp(1) *= thr_xy_max;
 
@@ -233,7 +233,7 @@ void PositionControl::_velocityController(const float &dt)
 		thrust_desired_NE(1) = MPC_XY_VEL_P.get() * vel_err(1) + MPC_XY_VEL_D.get() * _vel_dot(1) + _thr_int(1);
 
 		// Get maximum allowed thrust in NE based on tilt and excess thrust.
-		float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt_max);
+		float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
 		float thrust_max_NE = sqrtf(MPC_THR_MAX.get() * MPC_THR_MAX.get() - _thr_sp(2) * _thr_sp(2));
 		thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
 
@@ -262,19 +262,28 @@ void PositionControl::_velocityController(const float &dt)
 	}
 }
 
-void PositionControl::updateConstraints(const Controller::Constraints &constraints)
+void PositionControl::updateConstraints(const vehicle_constraints_s &constraints)
 {
 	_constraints = constraints;
 
-	// Check if adjustable constraints are below global constraints. If they are not stricter than global
+	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
 	// constraints, then just use global constraints for the limits.
 
-	if (!PX4_ISFINITE(constraints.tilt_max) || !(constraints.tilt_max < MPC_TILTMAX_AIR_rad.get())) {
+	if (!PX4_ISFINITE(constraints.tilt)
-		_constraints.tilt_max = MPC_TILTMAX_AIR_rad.get();
+	    || !(constraints.tilt < math::max(MPC_TILTMAX_AIR_rad.get(), MPC_MAN_TILT_MAX_rad.get()))) {
+		_constraints.tilt = math::max(MPC_TILTMAX_AIR_rad.get(), MPC_MAN_TILT_MAX_rad.get());
 	}
 
-	if (!PX4_ISFINITE(constraints.vel_max_z_up) || !(constraints.vel_max_z_up < MPC_Z_VEL_MAX_UP.get())) {
+	if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < MPC_Z_VEL_MAX_UP.get())) {
-		_constraints.vel_max_z_up = MPC_Z_VEL_MAX_UP.get();
+		_constraints.speed_up = MPC_Z_VEL_MAX_UP.get();
+	}
+
+	if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < MPC_Z_VEL_MAX_DN.get())) {
+		_constraints.speed_down = MPC_Z_VEL_MAX_DN.get();
+	}
+
+	if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < MPC_XY_VEL_MAX.get())) {
+		_constraints.speed_xy = MPC_XY_VEL_MAX.get();
 	}
 }
 

src/modules/mc_pos_control/PositionControl.hpp

@@ -41,23 +41,10 @@
 
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_constraints.h>
 #include <px4_module_params.h>
 #pragma once
 
-namespace Controller
-{
-/** Constraints that depends on mode and are lower
- * 	than the global limits.
- * 	tilt_max: Cannot exceed PI/2 radians
- * 	vel_max_z_up: Cannot exceed maximum global velocity upwards
- * @see MPC_TILTMAX_AIR
- * @see MPC_Z_VEL_MAX_DN
- */
-struct Constraints {
-	float tilt_max; /**< maximum tilt always below Pi/2 */
-	float vel_max_z_up; /**< maximum speed upwards always smaller than MPC_VEL_Z_MAX_UP */
-};
-}
 /**
  * 	Core Position-Control for MC.
  * 	This class contains P-controller for position and
@@ -108,7 +95,7 @@ public:
 	 * Set constraints that are stricter than the global limits.
 	 * @param constraints a PositionControl structure with supported constraints
 	 */
-	void updateConstraints(const Controller::Constraints &constraints);
+	void updateConstraints(const vehicle_constraints_s &constraints);
 
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
@@ -188,7 +175,7 @@ private:
 	float _yaw_sp{}; /**< desired yaw */
 	float _yawspeed_sp{}; /** desired yaw-speed */
 	matrix::Vector3f _thr_int{}; /**< thrust integral term */
-	Controller::Constraints _constraints{}; /**< variable constraints */
+	vehicle_constraints_s _constraints{}; /**< variable constraints */
 	bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
 
 	DEFINE_PARAMETERS(