Acc FF - Add low-pass filter to acceleration feedforward

src/modules/mc_pos_control/PositionControl.cpp

@@ -255,8 +255,24 @@ void PositionControl::_velocityController(const float &dt)
 	// 	 consideration of the desired thrust in D-direction. In addition, the thrust in
 	// 	 NE-direction is also limited by the maximum tilt.
 
+	// Filter acceleration feedforward
+	if (_param_mpc_a_ff_cutoff.get() > FLT_EPSILON) {
+		// Apply a first order IIR filter
+		const float omega_c = M_TWOPI_F * _param_mpc_a_ff_cutoff.get();
+		const float alpha =  omega_c * dt / (1 + omega_c * dt);
+		_acc_sp_filtered = alpha * _acc_sp + (1 - alpha) * _acc_sp_filtered;
+
+	} else if (_param_mpc_a_ff_cutoff.get() < -FLT_EPSILON){
+		// Disable feedforward
+		_acc_sp_filtered.zero();
+
+	} else {
+		// Do not filter
+		_acc_sp_filtered = _acc_sp;
+	}
+
 	// Acceleration to thrust feedforward conversion
-	const Vector3f thr_ff = _acc_sp * _param_mpc_thr_hover.get() / CONSTANTS_ONE_G;
+	const Vector3f thr_ff = _acc_sp_filtered * _param_mpc_thr_hover.get() / CONSTANTS_ONE_G;
 
 	const Vector3f vel_err = _vel_sp - _vel;
 

src/modules/mc_pos_control/PositionControl.hpp

@@ -221,6 +221,7 @@ class PositionControl : public ModuleParams
 	float _yaw_sp{}; /**< desired yaw */
 	float _yawspeed_sp{}; /** desired yaw-speed */
 	matrix::Vector3f _thr_int{}; /**< thrust integral term */
+	matrix::Vector3f _acc_sp_filtered{};
 	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 */
@@ -245,6 +246,7 @@ class PositionControl : public ModuleParams
 		(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
+		(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d,
+		(ParamFloat<px4::params::MPC_A_FF_CUTOFF>) _param_mpc_a_ff_cutoff
 	)
 };

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -233,6 +233,25 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
  */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
 
+/**
+ * Cutoff frequency for the low pass filter on the acceleration feedforward of the PID velocity controller
+ *
+ * The velocity controller can use acceleration as a feedforward to improve tracking performance. However,
+ * since this feedforward is directly translated to attitude setpoint, the signal need to be low-passed if
+ * it is not smooth.
+ *
+ * A value of 0 disables the filter.
+ * A value of -1 disables acceleration feedforward.
+ *
+ * @unit Hz
+ * @min -1
+ * @max 20
+ * @decimal 0
+ * @increment 1
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MPC_A_FF_CUTOFF, 10.f);
+
 /**
  * Maximum horizontal velocity in mission
  *