From e637e63d21ccf076bbe8852a981e8a0ca18a09ca Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Mon, 8 Aug 2022 17:56:45 +0200
Subject: [PATCH] do attitude estimate rotation in EKf for tailsitter in FW

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 src/modules/ekf2/EKF2.cpp                     | 35 +++++++++++++++++++
 src/modules/ekf2/EKF2.hpp                     |  2 ++
 .../FixedwingAttitudeControl.cpp              | 24 ++++++-------
 3 files changed, 49 insertions(+), 12 deletions(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 64b6329bef..cd285472ae 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -532,6 +532,7 @@ void EKF2::Run()
 
 				_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
 						vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+				_is_tailsitter_in_fw = is_fixed_wing && vehicle_status.is_vtol_tailsitter;
 			}
 		}
 
@@ -667,6 +668,40 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
 		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+
+		/* get current rotation matrix and euler angles from control state quaternions */
+		matrix::Dcmf R = matrix::Quatf(att.q);
+
+		if (_is_tailsitter_in_fw) {
+			/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
+			 * */
+			matrix::Dcmf R_adapted = R;		//modified rotation matrix
+
+			/* move z to x */
+			R_adapted(0, 0) = R(0, 2);
+			R_adapted(1, 0) = R(1, 2);
+			R_adapted(2, 0) = R(2, 2);
+
+			/* move x to z */
+			R_adapted(0, 2) = R(0, 0);
+			R_adapted(1, 2) = R(1, 0);
+			R_adapted(2, 2) = R(2, 0);
+
+			/* change direction of pitch (convert to right handed system) */
+			R_adapted(0, 0) = -R_adapted(0, 0);
+			R_adapted(1, 0) = -R_adapted(1, 0);
+			R_adapted(2, 0) = -R_adapted(2, 0);
+
+			/* fill in new attitude data */
+			R = R_adapted;
+
+			const matrix::Eulerf euler_angles(R);
+
+			const matrix::Quatf quat_new(matrix::Eulerf(euler_angles.phi(), euler_angles.theta(), euler_angles.psi()));
+
+			quat_new.copyTo(att.q);
+		}
+
 		_attitude_pub.publish(att);
 
 	}  else if (_replay_mode) {
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 9df018b3fe..e0376976dd 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -183,6 +183,8 @@ private:
 		}
 	}
 
+	bool _is_tailsitter_in_fw{false};
+
 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
 	 */
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index 94b01f2706..fdf81954af 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -319,20 +319,20 @@ void FixedwingAttitudeControl::Run()
 			 * */
 			matrix::Dcmf R_adapted = R;		//modified rotation matrix
 
-			/* move z to x */
-			R_adapted(0, 0) = R(0, 2);
-			R_adapted(1, 0) = R(1, 2);
-			R_adapted(2, 0) = R(2, 2);
+			// /* move z to x */
+			// R_adapted(0, 0) = R(0, 2);
+			// R_adapted(1, 0) = R(1, 2);
+			// R_adapted(2, 0) = R(2, 2);
 
-			/* move x to z */
-			R_adapted(0, 2) = R(0, 0);
-			R_adapted(1, 2) = R(1, 0);
-			R_adapted(2, 2) = R(2, 0);
+			// /* move x to z */
+			// R_adapted(0, 2) = R(0, 0);
+			// R_adapted(1, 2) = R(1, 0);
+			// R_adapted(2, 2) = R(2, 0);
 
-			/* change direction of pitch (convert to right handed system) */
-			R_adapted(0, 0) = -R_adapted(0, 0);
-			R_adapted(1, 0) = -R_adapted(1, 0);
-			R_adapted(2, 0) = -R_adapted(2, 0);
+			// /* change direction of pitch (convert to right handed system) */
+			// R_adapted(0, 0) = -R_adapted(0, 0);
+			// R_adapted(1, 0) = -R_adapted(1, 0);
+			// R_adapted(2, 0) = -R_adapted(2, 0);
 
 			/* fill in new attitude data */
 			R = R_adapted;