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());
@@ -413,6 +415,8 @@ void MulticopterPositionControl::Run()
 		}
 
 		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
+		_vehicle_acceleration_sub.update(&_vehicle_acceleration);
+		_vehicle_attitude_sub.update(&_vehicle_attitude);
 
 		if (_param_mpc_use_hte.get()) {
 			hover_thrust_estimate_s hte;
@@ -542,6 +546,8 @@ void MulticopterPositionControl::Run()
 				math::max(speed_down, 0.f));
 
 			_control.setInputSetpoint(_setpoint);
+			_control.setAttitude(Quatf(_vehicle_attitude.q));
+			_control.setBodySpecificForce(Vector3f(_vehicle_acceleration.xyz), dt);
 
 			// update states
 			if (!PX4_ISFINITE(_setpoint.position[2])

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -61,6 +61,8 @@
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -105,6 +107,8 @@ private:
 
 	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
@@ -130,6 +134,9 @@ private:
 		.landed = true,
 	};
 
+	vehicle_attitude_s _vehicle_attitude{};
+	vehicle_acceleration_s _vehicle_acceleration{};
+
 	DEFINE_PARAMETERS(
 		// Position Control
 		(ParamFloat<px4::params::MPC_XY_P>)         _param_mpc_xy_p,
@@ -140,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;
@@ -211,7 +211,12 @@ void PositionControl::_accelerationControl()
 		z_specific_force += _acc_sp(2);
 	}
 
-	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
+	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(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);
 	// Convert to thrust assuming hover thrust produces standard gravity
 	const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;

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,6 +150,9 @@ public:
 	 */
 	void setInputSetpoint(const trajectory_setpoint_s &setpoint);
 
+	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; }
+
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
 	 * thrust, yaw- and yawspeed-setpoints.
@@ -199,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
@@ -218,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 */
@@ -233,4 +242,6 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
+
+	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);