Merge pull request #156 from jgoppert/sensor_hil_rebase

Rebase of changes to sensor_hil_fixedwing branch.

1 files changed, 34 insertions, 41 deletions

diff --git a/apps/mathlib/math/Quaternion.cpp b/apps/mathlib/math/Quaternion.cpp
index 68fe85300..78481b286 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);
 }
 
@@ -142,38 +144,29 @@ int __EXPORT quaternionTest()
 	printf("Test Quaternion\t\t: ");
 	// test default ctor
 	Quaternion q;
-	ASSERT(equal(q.getA(), 1));
-	ASSERT(equal(q.getB(), 0));
-	ASSERT(equal(q.getC(), 0));
-	ASSERT(equal(q.getD(), 0));
+	ASSERT(equal(q.getA(), 1.0f));
+	ASSERT(equal(q.getB(), 0.0f));
+	ASSERT(equal(q.getC(), 0.0f));
+	ASSERT(equal(q.getD(), 0.0f));
 	// 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.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
+	ASSERT(equal(q.getA(), 0.1825742f));
+	ASSERT(equal(q.getB(), 0.3651484f));
+	ASSERT(equal(q.getC(), 0.5477226f));
+	ASSERT(equal(q.getD(), 0.7302967f));
 	// 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.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
 	// 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.0f, 0.0f, 0.0f, 0.0f)));
 	// 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));
+	q.setA(0.1f);
+	q.setB(0.2f);
+	q.setC(0.3f);
+	q.setD(0.4f);
+	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
 	printf("PASS\n");
 	return 0;
 }