mc_pos_control: enforce MPC_JERK_MAX in PositionControl

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -261,6 +261,9 @@ void MulticopterPositionControl::parameters_update(bool force)
 		_takeoff.setSpoolupTime(_param_com_spoolup_time.get());
 		_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
 		_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
+
+		// set position controller jerk limit to MPC_JERK_MAX + 10% margin to not interfere with reference trajectory
+		_control.setJerkLimit(_param_mpc_jerk_max.get() * 1.1f);
 	}
 }
 

src/modules/mc_pos_control/PositionControl/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_library(PositionControl
 	PositionControl.hpp
 )
 target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(PositionControl PRIVATE SlewRate)
 
 px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl)
 px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl)

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -146,7 +146,7 @@ void PositionControl::_velocityControl(const float dt)
 	// No control input from setpoints or corresponding states which are NAN
 	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
 
-	_accelerationControl();
+	_accelerationControl(dt);
 
 	// Integrator anti-windup in vertical direction
 	if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
@@ -201,8 +201,13 @@ void PositionControl::_velocityControl(const float dt)
 	_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));
 }
 
-void PositionControl::_accelerationControl()
+void PositionControl::_accelerationControl(const float dt)
 {
+	// don't allow accel setpoint to change faster than MPC_JERK_MAX m/s^2/s
+	for (int i = 0; i < 3; i++) {
+		_acc_sp(i) = _acc_sp_slew_rate[i].update(_acc_sp(i), dt);
+	}
+
 	// Assume standard acceleration due to gravity in vertical direction for attitude generation
 	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
 	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -40,6 +40,7 @@
 #pragma once
 
 #include <lib/mathlib/mathlib.h>
+#include <lib/slew_rate/SlewRate.hpp>
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -75,7 +76,11 @@ class PositionControl
 {
 public:
 
-	PositionControl() = default;
+	PositionControl()
+	{
+		setJerkLimit(INFINITY);
+	};
+
 	~PositionControl() = default;
 
 	/**
@@ -119,6 +124,13 @@ public:
 	 */
 	void setTiltLimit(const float tilt) { _lim_tilt = tilt; }
 
+	void setJerkLimit(const float jerk_max)
+	{
+		_acc_sp_slew_rate[0].setSlewRate(jerk_max);
+		_acc_sp_slew_rate[1].setSlewRate(jerk_max);
+		_acc_sp_slew_rate[2].setSlewRate(jerk_max);
+	}
+
 	/**
 	 * Set the normalized hover thrust
 	 * @param thrust [0.1, 0.9] with which the vehicle hovers not acelerating down or up with level orientation
@@ -183,7 +195,7 @@ private:
 
 	void _positionControl(); ///< Position proportional control
 	void _velocityControl(const float dt); ///< Velocity PID control
-	void _accelerationControl(); ///< Acceleration setpoint processing
+	void _accelerationControl(const float dt); ///< Acceleration setpoint processing
 
 	// Gains
 	matrix::Vector3f _gain_pos_p; ///< Position control proportional gain
@@ -216,4 +228,6 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
+
+	SlewRate<float> _acc_sp_slew_rate[3];
 };