mpc: generate tilt based on specific force feedback

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -413,6 +413,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 +544,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));
 
 			// 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,

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -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(_body_specific_force, sf_sp_body);
+	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

@@ -146,6 +146,9 @@ public:
 	 */
 	void setInputSetpoint(const trajectory_setpoint_s &setpoint);
 
+	void setBodySpecificForce(const matrix::Vector3f &specific_force) { _body_specific_force = specific_force; }
+	void setAttitude(const matrix::Quatf &attitude) { _attitude = attitude; }
+
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
 	 * thrust, yaw- and yawspeed-setpoints.
@@ -233,4 +236,7 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
+
+	matrix::Vector3f _body_specific_force;
+	matrix::Quatf _attitude;
 };