From a91a35685ad84c5ba4d89834a2d142a1dd6fee94 Mon Sep 17 00:00:00 2001
From: corvus <corvus>
Date: Thu, 17 Feb 2011 01:51:45 +0000
Subject: [PATCH] IL2 add quaternion attitude and accels

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2802 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 .../src/libs/utils/coordinateconversions.cpp  | 51 +++++++++++++++++++
 .../src/libs/utils/coordinateconversions.h    |  2 +
 .../src/plugins/hitlnew/il2simulator.cpp      | 17 +++++--
 3 files changed, 66 insertions(+), 4 deletions(-)

diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
index 67017283f..1ca010dbd 100644
--- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
+++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
@@ -167,4 +167,55 @@ void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rn
         NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
 }
 
+// ****** find roll, pitch, yaw from quaternion ********
+void CoordinateConversions::Quaternion2RPY(const float q[4], float rpy[3])
+{
+	float R13, R11, R12, R23, R33;
+	float q0s = q[0] * q[0];
+	float q1s = q[1] * q[1];
+	float q2s = q[2] * q[2];
+	float q3s = q[3] * q[3];
+
+	R13 = 2 * (q[1] * q[3] - q[0] * q[2]);
+	R11 = q0s + q1s - q2s - q3s;
+	R12 = 2 * (q[1] * q[2] + q[0] * q[3]);
+	R23 = 2 * (q[2] * q[3] + q[0] * q[1]);
+	R33 = q0s - q1s - q2s + q3s;
+
+	rpy[1] = RAD2DEG * asinf(-R13);	// pitch always between -pi/2 to pi/2
+	rpy[2] = RAD2DEG * atan2f(R12, R11);
+	rpy[0] = RAD2DEG * atan2f(R23, R33);
+
+	//TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
+}
+
+// ****** find quaternion from roll, pitch, yaw ********
+void CoordinateConversions::RPY2Quaternion(const float rpy[3], float q[4])
+{
+	float phi, theta, psi;
+	float cphi, sphi, ctheta, stheta, cpsi, spsi;
+
+	phi = DEG2RAD * rpy[0] / 2;
+	theta = DEG2RAD * rpy[1] / 2;
+	psi = DEG2RAD * rpy[2] / 2;
+	cphi = cosf(phi);
+	sphi = sinf(phi);
+	ctheta = cosf(theta);
+	stheta = sinf(theta);
+	cpsi = cosf(psi);
+	spsi = sinf(psi);
+
+	q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi;
+	q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi;
+	q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi;
+	q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi;
+
+	if (q[0] < 0) {		// q0 always positive for uniqueness
+		q[0] = -q[0];
+		q[1] = -q[1];
+		q[2] = -q[2];
+		q[3] = -q[3];
+	}
+}
+
 }
diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h
index 0bb7a4571..1b469f15b 100644
--- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h
+++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h
@@ -46,6 +46,8 @@ public:
     void LLA2ECEF(double LLA[3], double ECEF[3]);
     int ECEF2LLA(double ECEF[3], double LLA[3]);
     void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
+    void Quaternion2RPY(const float q[4], float rpy[3]);
+    void RPY2Quaternion(const float rpy[3], float q[4]);
 };
 
 }
diff --git a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
index 96cf950f8..d04eb3c2b 100644
--- a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
+++ b/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
@@ -249,10 +249,16 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
         attActualData.Roll = current.roll;
         attActualData.Pitch = current.pitch;
         attActualData.Yaw = current.azimuth;
-        attActualData.q1 = 0;
-        attActualData.q2 = 0;
-        attActualData.q3 = 0;
-        attActualData.q4 = 0;
+	float rpy[3];
+	float quat[4];
+	rpy[0]=current.roll;
+	rpy[1]=current.pitch;
+	rpy[2]=current.yaw;
+        Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
+        attActualData.q1 = quat[0];
+        attActualData.q2 = quat[1];
+        attActualData.q3 = quat[2];
+        attActualData.q4 = quat[3];
 
         // Update positionActual objects
         PositionActual::DataFields posData;
@@ -275,6 +281,9 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
         rawData.gyros[0] = current.dRoll;
         rawData.gyros[1] = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
         rawData.gyros[2] = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
+	rawDara.accels[0] = current.ddX;
+	rawDara.accels[1] = current.ddY;
+	rawDara.accels[2] = current.ddZ;
 
         // Update homelocation
         HomeLocation::DataFields homeData;