Rebase of changes to sensor_hil_fixedwing branch.

1 files changed, 26 insertions, 33 deletions

diff --git a/apps/mathlib/math/Quaternion.cpp b/apps/mathlib/math/Quaternion.cpp
index 68fe85300..4f07840c3 100644
--- a/apps/mathlib/math/Quaternion.cpp
+++ b/apps/mathlib/math/Quaternion.cpp
@@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) :
 Quaternion::Quaternion(const Dcm &dcm) :
 	Vector(4)
 {
-	setA(0.5f * sqrtf(1 + dcm(0, 0) +
-			  dcm(1, 1) + dcm(2, 2)));
-	setB((dcm(2, 1) - dcm(1, 2)) /
-	     (4 * getA()));
-	setC((dcm(0, 2) - dcm(2, 0)) /
-	     (4 * getA()));
-	setD((dcm(1, 0) - dcm(0, 1)) /
-	     (4 * getA()));
+	// avoiding singularities by not using
+	// division equations
+	setA(0.5 * sqrt(1.0 +
+		double( dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
+	setB(0.5 * sqrt(1.0 +
+		double( dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
+	setC(0.5 * sqrt(1.0 +
+		double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
+	setD(0.5 * sqrt(1.0 +
+		double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
 }
 
 Quaternion::Quaternion(const EulerAngles &euler) :
 	Vector(4)
 {
-	float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
-	float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
-	float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
-	float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
-	float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
-	float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
+	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
+	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
+	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
+	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
+	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
+	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
 	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
 	     sinPhi_2 * sinTheta_2 * sinPsi_2);
 	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
 	     cosPhi_2 * sinTheta_2 * sinPsi_2);
 	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
 	     sinPhi_2 * cosTheta_2 * sinPsi_2);
-	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
+	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
 	     sinPhi_2 * sinTheta_2 * cosPsi_2);
 }
 
@@ -147,33 +149,24 @@ int __EXPORT quaternionTest()
 	ASSERT(equal(q.getC(), 0));
 	ASSERT(equal(q.getD(), 0));
 	// test float ctor
-	q = Quaternion(0, 1, 0, 0);
-	ASSERT(equal(q.getA(), 0));
-	ASSERT(equal(q.getB(), 1));
-	ASSERT(equal(q.getC(), 0));
-	ASSERT(equal(q.getD(), 0));
+	q = Quaternion(0.1825742, 0.3651484, 0.5477226, 0.7302967);
+	ASSERT(equal(q.getA(), 0.1825742));
+	ASSERT(equal(q.getB(), 0.3651484));
+	ASSERT(equal(q.getC(), 0.5477226));
+	ASSERT(equal(q.getD(), 0.7302967));
 	// test euler ctor
-	q = Quaternion(EulerAngles(0, 0, 0));
-	ASSERT(equal(q.getA(), 1));
-	ASSERT(equal(q.getB(), 0));
-	ASSERT(equal(q.getC(), 0));
-	ASSERT(equal(q.getD(), 0));
+	q = Quaternion(EulerAngles(0.1, 0.2, 0.3));
+	ASSERT(vectorEqual(q, Quaternion(0.983347, 0.034271, 0.106021, 0.143572)));
 	// test dcm ctor
 	q = Quaternion(Dcm());
-	ASSERT(equal(q.getA(), 1));
-	ASSERT(equal(q.getB(), 0));
-	ASSERT(equal(q.getC(), 0));
-	ASSERT(equal(q.getD(), 0));
+	ASSERT(vectorEqual(q, Quaternion(1, 0, 0, 0)));
 	// TODO test derivative
 	// test accessors
 	q.setA(0.1);
 	q.setB(0.2);
 	q.setC(0.3);
 	q.setD(0.4);
-	ASSERT(equal(q.getA(), 0.1));
-	ASSERT(equal(q.getB(), 0.2));
-	ASSERT(equal(q.getC(), 0.3));
-	ASSERT(equal(q.getD(), 0.4));
+	ASSERT(vectorEqual(q, Quaternion(0.1, 0.2, 0.3, 0.4)));
 	printf("PASS\n");
 	return 0;
 }