PositionControl: remove parameter dependency

to make the class easier to understand,
more modular and easily unit testable

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -43,9 +43,25 @@
 
 using namespace matrix;
 
-PositionControl::PositionControl(ModuleParams *parent) :
-	ModuleParams(parent)
-{}
+void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+{
+	_gain_vel_p = P;
+	_gain_vel_i = I;
+	_gain_vel_d = D;
+}
+
+void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down)
+{
+	_lim_vel_horizontal = vel_horizontal;
+	_lim_vel_up = vel_up;
+	_lim_vel_down = vel_down;
+}
+
+void PositionControl::setThrustLimits(const float min, const float max)
+{
+	_lim_thr_min = min;
+	_lim_thr_max = max;
+}
 
 void PositionControl::updateState(const PositionControlStates &states)
 {
@@ -91,17 +107,17 @@ void PositionControl::generateThrustYawSetpoint(const float dt)
 		// Limit the thrust vector.
 		float thr_mag = _thr_sp.length();
 
-		if (thr_mag > _param_mpc_thr_max.get()) {
-			_thr_sp = _thr_sp.normalized() * _param_mpc_thr_max.get();
+		if (thr_mag > _lim_thr_max) {
+			_thr_sp = _thr_sp.normalized() * _lim_thr_max;
 
-		} else if (thr_mag < _param_mpc_manthr_min.get() && thr_mag > FLT_EPSILON) {
-			_thr_sp = _thr_sp.normalized() * _param_mpc_manthr_min.get();
+		} else if (thr_mag < _lim_thr_min && thr_mag > FLT_EPSILON) {
+			_thr_sp = _thr_sp.normalized() * _lim_thr_min;
 		}
 
 		// Just set the set-points equal to the current vehicle state.
 		_pos_sp = _pos;
 		_vel_sp = _vel;
-		_acc_sp = _acc;
+		_acc_sp = _vel_dot;
 
 	} else {
 		_positionController();
@@ -204,7 +220,7 @@ bool PositionControl::_interfaceMapping()
 		_thr_sp(0) = _thr_sp(1) = 0.0f;
 		// throttle down such that vehicle goes down with
 		// 70% of throttle range between min and hover
-		_thr_sp(2) = -(_param_mpc_thr_min.get() + (_param_mpc_thr_hover.get() - _param_mpc_thr_min.get()) * 0.7f);
+		_thr_sp(2) = -(_lim_thr_min + (_hover_thrust - _lim_thr_min) * 0.7f);
 		// position and velocity control-loop is currently unused (flag only for logging purpose)
 		_setCtrlFlag(false);
 	}
@@ -215,14 +231,13 @@ bool PositionControl::_interfaceMapping()
 void PositionControl::_positionController()
 {
 	// P-position controller
-	const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
-					 _param_mpc_z_p.get()));
+	const Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
 	_vel_sp = vel_sp_position + _vel_sp;
 
 	// Constrain horizontal velocity by prioritizing the velocity component along the
 	// the desired position setpoint over the feed-forward term.
 	const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
-				   Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
+				   Vector2f(_vel_sp - vel_sp_position), _lim_vel_horizontal);
 	_vel_sp(0) = vel_sp_xy(0);
 	_vel_sp(1) = vel_sp_xy(1);
 	// Constrain velocity in z-direction.
@@ -258,12 +273,12 @@ void PositionControl::_velocityController(const float &dt)
 	const Vector3f vel_err = _vel_sp - _vel;
 
 	// Consider thrust in D-direction.
-	float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
-					 2) - _param_mpc_thr_hover.get();
+	float thrust_desired_D = _gain_vel_p(2) * vel_err(2) +  _gain_vel_d(2) * _vel_dot(2) + _thr_int(
+					 2) - _hover_thrust;
 
 	// The Thrust limits are negated and swapped due to NED-frame.
-	float uMax = -_param_mpc_thr_min.get();
-	float uMin = -_param_mpc_thr_max.get();
+	float uMax = -_lim_thr_min;
+	float uMin = -_lim_thr_max;
 
 	// make sure there's always enough thrust vector length to infer the attitude
 	uMax = math::min(uMax, -10e-4f);
@@ -273,14 +288,15 @@ void PositionControl::_velocityController(const float &dt)
 			       (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);
 
 	if (!stop_integral_D) {
-		_thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;
+		_thr_int(2) += vel_err(2) * _gain_vel_i(2) * dt;
 
 		// limit thrust integral
-		_thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
+		_thr_int(2) = math::min(fabsf(_thr_int(2)), _lim_thr_max) * math::sign(_thr_int(2));
 	}
 
 	// Saturate thrust setpoint in D-direction.
 	_thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);
+	printf("%.3f %.3f\n", (double)uMin, (double)uMax);
 
 	if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
 		// Thrust set-point in NE-direction is already provided. Only
@@ -292,12 +308,13 @@ void PositionControl::_velocityController(const float &dt)
 	} else {
 		// PID-velocity controller for NE-direction.
 		Vector2f thrust_desired_NE;
-		thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
-		thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);
+		thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _thr_int(0);
+		thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _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);
-		float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
+		float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2));
+		printf("%.3f %.3f\n", (double)(_lim_thr_max), (double)(_thr_sp(2)));
 		thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
 
 		// Saturate thrust in NE-direction.
@@ -312,15 +329,15 @@ void PositionControl::_velocityController(const float &dt)
 
 		// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
 		// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
-		float arw_gain = 2.f / _param_mpc_xy_vel_p.get();
+		float arw_gain = 2.f / _gain_vel_p(0);
 
 		Vector2f vel_err_lim;
 		vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
 		vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;
 
 		// Update integral
-		_thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
-		_thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
+		_thr_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt;
+		_thr_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt;
 	}
 }
 
@@ -331,23 +348,21 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
 	// 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.
 
-	const float tilt_max_radians = math::radians(math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()));
-
 	if (!PX4_ISFINITE(constraints.tilt)
-	    || !(constraints.tilt < tilt_max_radians)) {
-		_constraints.tilt = tilt_max_radians;
+	    || !(constraints.tilt < _lim_tilt)) {
+		_constraints.tilt = _lim_tilt;
 	}
 
-	if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) {
-		_constraints.speed_up = _param_mpc_z_vel_max_up.get();
+	if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _lim_vel_up)) {
+		_constraints.speed_up = _lim_vel_up;
 	}
 
-	if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _param_mpc_z_vel_max_dn.get())) {
-		_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+	if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _lim_vel_down)) {
+		_constraints.speed_down = _lim_vel_down;
 	}
 
-	if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _param_mpc_xy_vel_max.get())) {
-		_constraints.speed_xy = _param_mpc_xy_vel_max.get();
+	if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _lim_vel_horizontal)) {
+		_constraints.speed_xy = _lim_vel_horizontal;
 	}
 }
 
@@ -370,8 +385,3 @@ void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_
 	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }
-
-void PositionControl::updateParams()
-{
-	ModuleParams::updateParams();
-}

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -37,14 +37,13 @@
  * A cascaded position controller for position/velocity control only.
  */
 
-#include <matrix/matrix/math.hpp>
+#pragma once
 
+#include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_constraints.h>
-#include <px4_platform_common/module_params.h>
-#pragma once
 
 struct PositionControlStates {
 	matrix::Vector3f position;
@@ -73,20 +72,53 @@ struct PositionControlStates {
  * 	If there is a position/velocity- and thrust-setpoint present, then
  *  the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop.
  */
-class PositionControl : public ModuleParams
+class PositionControl
 {
 public:
 
-	PositionControl(ModuleParams *parent);
+	PositionControl() = default;
 	~PositionControl() = default;
 
 	/**
-	 *	Overwrites certain parameters.
-	 *	Overwrites are required for unit-conversion.
-	 *	This method should only be called if parameters
-	 *	have been updated.
+	 * Set the position control gains
+	 * @param P 3D vector of proportional gains for x,y,z axis
 	 */
-	void overwriteParams();
+	void setPositionGains(const matrix::Vector3f &P) { _gain_pos_p = P; }
+
+	/**
+	 * Set the velocity control gains
+	 * @param P 3D vector of proportional gains for x,y,z axis
+	 * @param I 3D vector of integral gains
+	 * @param D 3D vector of derivative gains
+	 */
+	void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+
+	/**
+	 * Set the maximum velocity to execute with feed forward and position control
+	 * @param vel_horizontal horizontal velocity limit
+	 * @param vel_up upwards velocity limit
+	 * @param vel_down downwards velocity limit
+	 */
+	void setVelocityLimits(const float vel_horizontal, const float vel_up, float vel_down);
+
+	/**
+	 * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller
+	 * @param min minimum thrust e.g. 0.1 or 0
+	 * @param max maximum thrust e.g. 0.9 or 1
+	 */
+	void setThrustLimits(const float min, const float max);
+
+	/**
+	 * Set the maximum tilt angle in radians the output attitude is allowed to have
+	 * @param tilt angle from level orientation in radians
+	 */
+	void setTiltLimit(const float tilt) { _lim_tilt = tilt; }
+
+	/**
+	 * Set the maximum tilt angle in radians the output attitude is allowed to have
+	 * @param thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation
+	 */
+	void setHoverThrust(const float thrust) { _hover_thrust = thrust; }
 
 	/**
 	 * Update the current vehicle state.
@@ -166,10 +198,6 @@ public:
 	 */
 	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
-protected:
-
-	void updateParams() override;
-
 private:
 	/**
 	 * Maps setpoints to internal-setpoints.
@@ -181,42 +209,40 @@ private:
 	void _velocityController(const float &dt); /** applies the PID-velocity-controller */
 	void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */
 
-	matrix::Vector3f _pos{}; /**< MC position */
-	matrix::Vector3f _vel{}; /**< MC velocity */
-	matrix::Vector3f _vel_dot{}; /**< MC velocity derivative */
-	matrix::Vector3f _acc{}; /**< MC acceleration */
-	float _yaw{0.0f}; /**< MC yaw */
-	matrix::Vector3f _pos_sp{}; /**< desired position */
-	matrix::Vector3f _vel_sp{}; /**< desired velocity */
-	matrix::Vector3f _acc_sp{}; /**< desired acceleration: not supported yet */
-	matrix::Vector3f _thr_sp{}; /**< desired thrust */
+	// Gains
+	matrix::Vector3f _gain_pos_p; ///< Position control proportional gain
+	matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain
+	matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain
+	matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain
+
+	// Limits
+	float _lim_vel_horizontal{0}; ///< Horizontal velocity limit with feed forward and position control
+	float _lim_vel_up{0}; ///< Upwards velocity limit with feed forward and position control
+	float _lim_vel_down{0}; ///< Downwards velocity limit with feed forward and position control
+	float _lim_thr_min{0}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9
+	float _lim_thr_max{0}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
+	float _lim_tilt{0}; ///< Maximum tilt from level the output attitude is allowed to have
+
+	float _hover_thrust{0}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation
+
+	// States
+	matrix::Vector3f _pos; /**< position */
+	matrix::Vector3f _vel; /**< velocity */
+	matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */
+	matrix::Vector3f _thr_int; /**< integral term of the velocity controller */
+	float _yaw = 0.0f; /**< yaw */
+
+	vehicle_constraints_s _constraints{}; /**< variable constraints */
+
+	// Setpoints
+	matrix::Vector3f _pos_sp; /**< desired position */
+	matrix::Vector3f _vel_sp; /**< desired velocity */
+	matrix::Vector3f _acc_sp; /**< desired acceleration */
+	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired yaw */
 	float _yawspeed_sp{}; /** desired yaw-speed */
-	matrix::Vector3f _thr_int{}; /**< thrust integral term */
-	vehicle_constraints_s _constraints{}; /**< variable constraints */
+
 	bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
 	bool _ctrl_pos[3] = {true, true, true}; /**< True if the control-loop for position was used */
 	bool _ctrl_vel[3] = {true, true, true}; /**< True if the control-loop for velocity was used */
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
-		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
-		(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
-		(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min,
-		(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
-		(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
-		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
-		(ParamFloat<px4::params::MPC_TILTMAX_AIR>)
-		_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in degrees
-		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
-		_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in degrees
-		(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
-		(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
-		(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,
-		(ParamFloat<px4::params::MPC_Z_VEL_D>) _param_mpc_z_vel_d,
-		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
-		(ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
-		(ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
-		(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d
-	)
 };

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -146,24 +146,36 @@ private:
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
+		// Position Control
+		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
+		(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
+		(ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
+		(ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
+		(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d,
+		(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
+		(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,
+		(ParamFloat<px4::params::MPC_Z_VEL_D>) _param_mpc_z_vel_d,
 		(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
-		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
-		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
 		(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_LAND_SPEED>) _param_mpc_land_speed,
+		(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
+		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
+
+		// Takeoff / Land
+		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
+		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
 		(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
+		(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+
+		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
 		(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
 		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
 		(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
 		(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
-		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
 		(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
-		(ParamFloat<px4::params::MPC_THR_MIN>)_param_mpc_thr_min,
-		(ParamFloat<px4::params::MPC_THR_HOVER>)_param_mpc_thr_hover,
-		(ParamFloat<px4::params::MPC_THR_MAX>)_param_mpc_thr_max,
-		(ParamFloat<px4::params::MPC_Z_VEL_P>)_param_mpc_z_vel_p
+		(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
+		(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max
 	);
 
 	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -275,7 +287,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel_x_deriv(this, "VELD"),
 	_vel_y_deriv(this, "VELD"),
 	_vel_z_deriv(this, "VELD"),
-	_control(this),
 	_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time"))
 {
 	// fetch initial parameter values
@@ -331,6 +342,15 @@ MulticopterPositionControl::parameters_update(bool force)
 		ModuleParams::updateParams();
 		SuperBlock::updateParams();
 
+		_control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get()));
+		_control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()),
+					  Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()),
+					  Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()));
+		_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
+		_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
+		_control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!
+		_control.setHoverThrust(_param_mpc_thr_hover.get());
+
 		// Check that the design parameters are inside the absolute maximum constraints
 		if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {
 			_param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get());