From 923b5b15bdac3f69fda54fa3cfe0dd92037f22ba Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 23 May 2022 08:17:47 +0200
Subject: [PATCH] TEST: allow omnicopter to rotate via AUX1 & use 3D thrust in
 pos/mission mode

---
 .../MulticopterPositionControl.cpp            |  2 +-
 .../PositionControl/PositionControl.cpp       | 27 +++++++++++++++++--
 .../PositionControl/PositionControl.hpp       |  8 +++++-
 3 files changed, 33 insertions(+), 4 deletions(-)

diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
index 47e1aecf2d..bc43d156ed 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
@@ -531,7 +531,7 @@ void MulticopterPositionControl::Run()
 
 			// Publish attitude setpoint output
 			vehicle_attitude_setpoint_s attitude_setpoint{};
-			_control.getAttitudeSetpoint(attitude_setpoint);
+			_control.getAttitudeSetpoint(attitude_setpoint, _vehicle_land_detected.landed);
 			attitude_setpoint.timestamp = hrt_absolute_time();
 			_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
 
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 8dd2c521e7..1984d44ee2 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -117,6 +117,7 @@ bool PositionControl::update(const float dt)
 	// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
 	valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
 	valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
+	_dt = dt;
 
 	return valid;
 }
@@ -249,8 +250,30 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }
 
-void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
+void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, bool landed)
 {
-	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
+	// ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
+
+
+	manual_control_setpoint_s manual_control_setpoint;
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	if (landed) {
+		_roll_angle = 0.f;
+		_pitch_angle = 0.f;
+
+	} else {
+		_roll_angle += _dt * manual_control_setpoint.aux1 * 2.f * M_PI_F / 2.f;
+		// _pitch_angle += _dt * powf(manual_control_setpoint.aux1, 2.f) * 2.f * M_PI_F / 2.f; // TODO: aux2
+	}
+
+	Quatf q_sp = Eulerf(_roll_angle, _pitch_angle, _yaw_sp);
+	q_sp.copyTo(attitude_setpoint.q_d);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
+
+	// Rotate thrust by negative attitude
+	Dcmf att_sp_dcm{q_sp};
+	Vector3f thrust_sp_body = att_sp_dcm.transpose() * _thr_sp;
+	thrust_sp_body.copyTo(attitude_setpoint.thrust_body);
+
 }
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
index fa2fd35f80..9c93c5f89e 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
@@ -43,6 +43,8 @@
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/Subscription.hpp>
 
 struct PositionControlStates {
 	matrix::Vector3f position;
@@ -176,7 +178,7 @@ public:
 	 * It needs to be executed by the attitude controller to achieve velocity and position tracking.
 	 * @param attitude_setpoint reference to struct to fill up
 	 */
-	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
+	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, bool landed);
 
 private:
 	bool _inputValid();
@@ -216,4 +218,8 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
+	float _roll_angle{0.f};
+	float _pitch_angle{0.f};
+	float _dt{0.f};
 };