mpc: add acceleration control filter and gain

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -198,6 +198,8 @@ void MulticopterPositionControl::parameters_update(bool force)
 			Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()),
 			Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()),
 			Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get()));
+		_control.setAccelerationGains(_param_mpc_xy_acc_p.get());
+		_control.setAccelerationCutoff(_param_mpc_acc_cutoff.get());
 		_control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get());
 		_control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get());
 		_goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get());
@@ -545,7 +547,7 @@ void MulticopterPositionControl::Run()
 
 			_control.setInputSetpoint(_setpoint);
 			_control.setAttitude(Quatf(_vehicle_attitude.q));
-			_control.setBodySpecificForce(Vector3f(_vehicle_acceleration.xyz));
+			_control.setBodySpecificForce(Vector3f(_vehicle_acceleration.xyz), dt);
 
 			// update states
 			if (!PX4_ISFINITE(_setpoint.position[2])

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -147,6 +147,8 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>)  _param_mpc_z_vel_p_acc,
 		(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_ACC_P>)     _param_mpc_xy_acc_p,
+		(ParamFloat<px4::params::MPC_ACC_CUTOFF>)   _param_mpc_acc_cutoff,
 		(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,

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -149,7 +149,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.f) ||
@@ -201,7 +201,7 @@ void PositionControl::_velocityControl(const float dt)
 	_vel_int += vel_error.emult(_gain_vel_i) * dt;
 }
 
-void PositionControl::_accelerationControl()
+void PositionControl::_accelerationControl(const float dt)
 {
 	// Assume standard acceleration due to gravity in vertical direction for attitude generation
 	float z_specific_force = -CONSTANTS_ONE_G;
@@ -214,7 +214,7 @@ void PositionControl::_accelerationControl()
 	const Vector3f sf_sp_ned = Vector3f(_acc_sp(0), _acc_sp(1), z_specific_force);
 	const Vector3f sf_sp_body = _attitude.rotateVectorInverse(sf_sp_ned);
 
-	Quatf att_error(_body_specific_force, sf_sp_body);
+	Quatf att_error(AxisAnglef(AxisAnglef(Quatf(_body_specific_force_lpf.getState(), sf_sp_body)) * _gain_acc_xy));
 	Quatf desired_att = _attitude * att_error;
 	Vector3f body_z = desired_att.dcm_z();
 	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 <mathlib/math/filter/AlphaFilter.hpp>
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -93,6 +94,9 @@ public:
 	 */
 	void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
 
+	void setAccelerationGains(const float gain_xy) { _gain_acc_xy = gain_xy; }
+	void setAccelerationCutoff(const float cutoff_frequency) { _body_specific_force_lpf.setCutoffFreq(cutoff_frequency); }
+
 	/**
 	 * Set the maximum velocity to execute with feed forward and position control
 	 * @param vel_horizontal horizontal velocity limit
@@ -146,7 +150,7 @@ public:
 	 */
 	void setInputSetpoint(const trajectory_setpoint_s &setpoint);
 
-	void setBodySpecificForce(const matrix::Vector3f &specific_force) { _body_specific_force = specific_force; }
+	void setBodySpecificForce(const matrix::Vector3f &specific_force, const float dt) { _body_specific_force_lpf.update(specific_force, dt); }
 	void setAttitude(const matrix::Quatf &attitude) { _attitude = attitude; }
 
 	/**
@@ -202,7 +206,7 @@ private:
 
 	void _positionControl(); ///< Position proportional control
 	void _velocityControl(const float dt); ///< Velocity PID control
-	void _accelerationControl(); ///< Acceleration setpoint processing
+	void _accelerationControl(float dt); ///< Acceleration setpoint processing
 
 	// Gains
 	matrix::Vector3f _gain_pos_p; ///< Position control proportional gain
@@ -221,6 +225,8 @@ private:
 
 	float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation
 	bool _decouple_horizontal_and_vertical_acceleration{true}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint
+	float _gain_acc_xy{1.f};
+	AlphaFilter<matrix::Vector3f> _body_specific_force_lpf{};
 
 	// States
 	matrix::Vector3f _pos; /**< current position */
@@ -237,6 +243,5 @@ private:
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
 
-	matrix::Vector3f _body_specific_force;
 	matrix::Quatf _attitude;
 };

src/modules/mc_pos_control/multicopter_position_control_gain_params.c

@@ -135,3 +135,29 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.f);
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f);
+
+/**
+ * Gain for xy acceleration control
+ *
+ * Reduce to lower the sensitivity to accelerometer noise
+ *
+ * @min 0.1
+ * @max 1
+ * @decimal 2
+ * @increment 0.05
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_ACC_P, 1.f);
+
+/**
+ * Acceleration feedback cutoff frequency
+ *
+ * 0 to disable
+ *
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_ACC_CUTOFF, 0.1f);