Rebase of changes to sensor_hil_fixedwing branch.

6 files changed, 164 insertions, 68 deletions

diff --git a/apps/mathlib/math/Dcm.cpp b/apps/mathlib/math/Dcm.cpp
index 59f3d88ab..467c6f34e 100644
--- a/apps/mathlib/math/Dcm.cpp
+++ b/apps/mathlib/math/Dcm.cpp
@@ -52,6 +52,23 @@ Dcm::Dcm() :
 {
 }
 
+Dcm::Dcm(float c00, float c01, float c02,
+	float c10, float c11, float c12,
+	float c20, float c21, float c22) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	dcm(0,0) = c00;
+	dcm(0,1) = c01;
+	dcm(0,2) = c02;
+	dcm(1,0) = c10;
+	dcm(1,1) = c11;
+	dcm(1,2) = c12;
+	dcm(2,0) = c20;
+	dcm(2,1) = c21;
+	dcm(2,2) = c22;
+}
+
 Dcm::Dcm(const float *data) :
 	Matrix(3, 3, data)
 {
@@ -61,22 +78,22 @@ Dcm::Dcm(const Quaternion &q) :
 	Matrix(3, 3)
 {
 	Dcm &dcm = *this;
-	float a = q.getA();
-	float b = q.getB();
-	float c = q.getC();
-	float d = q.getD();
-	float aSq = a * a;
-	float bSq = b * b;
-	float cSq = c * c;
-	float dSq = d * d;
+	double a = q.getA();
+	double b = q.getB();
+	double c = q.getC();
+	double d = q.getD();
+	double aSq = a * a;
+	double bSq = b * b;
+	double cSq = c * c;
+	double dSq = d * d;
 	dcm(0, 0) = aSq + bSq - cSq - dSq;
-	dcm(0, 1) = 2 * (b * c - a * d);
-	dcm(0, 2) = 2 * (a * c + b * d);
-	dcm(1, 0) = 2 * (b * c + a * d);
+	dcm(0, 1) = 2.0 * (b * c - a * d);
+	dcm(0, 2) = 2.0 * (a * c + b * d);
+	dcm(1, 0) = 2.0 * (b * c + a * d);
 	dcm(1, 1) = aSq - bSq + cSq - dSq;
-	dcm(1, 2) = 2 * (c * d - a * b);
-	dcm(2, 0) = 2 * (b * d - a * c);
-	dcm(2, 1) = 2 * (a * b + c * d);
+	dcm(1, 2) = 2.0 * (c * d - a * b);
+	dcm(2, 0) = 2.0 * (b * d - a * c);
+	dcm(2, 1) = 2.0 * (a * b + c * d);
 	dcm(2, 2) = aSq - bSq - cSq + dSq;
 }
 
@@ -84,12 +101,12 @@ Dcm::Dcm(const EulerAngles &euler) :
 	Matrix(3, 3)
 {
 	Dcm &dcm = *this;
-	float cosPhi = cosf(euler.getPhi());
-	float sinPhi = sinf(euler.getPhi());
-	float cosThe = cosf(euler.getTheta());
-	float sinThe = sinf(euler.getTheta());
-	float cosPsi = cosf(euler.getPsi());
-	float sinPsi = sinf(euler.getPsi());
+	double cosPhi = cos(euler.getPhi());
+	double sinPhi = sin(euler.getPhi());
+	double cosThe = cos(euler.getTheta());
+	double sinThe = sin(euler.getTheta());
+	double cosPsi = cos(euler.getPsi());
+	double sinPsi = sin(euler.getPsi());
 
 	dcm(0, 0) = cosThe * cosPsi;
 	dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
@@ -116,11 +133,23 @@ Dcm::~Dcm()
 int __EXPORT dcmTest()
 {
 	printf("Test DCM\t\t: ");
-	Vector3 vB(1, 2, 3);
-	ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
-			   Matrix::identity(3)));
-	ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
+	// default ctor
+	ASSERT(matrixEqual(Dcm(),
 			   Matrix::identity(3)));
+	// quaternion ctor
+	ASSERT(matrixEqual(
+	 	Dcm(Quaternion(0.983347, 0.034271, 0.106021, 0.143572)),
+		Dcm( 0.9362934, -0.2750958,  0.2183507,  
+ 			 0.2896295,  0.9564251, -0.0369570, 
+			-0.1986693,  0.0978434,  0.9751703)));
+	// euler angle ctor
+	ASSERT(matrixEqual(
+		Dcm(EulerAngles(0.1, 0.2, 0.3)),
+		Dcm( 0.9362934, -0.2750958,  0.2183507,  
+ 			 0.2896295,  0.9564251, -0.0369570, 
+			-0.1986693,  0.0978434,  0.9751703)));
+	// rotations
+	Vector3 vB(1, 2, 3);
 	ASSERT(vectorEqual(Vector3(-2, 1, 3),
 			   Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
 	ASSERT(vectorEqual(Vector3(3, 2, -1),
diff --git a/apps/mathlib/math/Dcm.hpp b/apps/mathlib/math/Dcm.hpp
index de69a7aa4..781933e9e 100644
--- a/apps/mathlib/math/Dcm.hpp
+++ b/apps/mathlib/math/Dcm.hpp
@@ -65,6 +65,13 @@ public:
 	Dcm();
 
 	/**
+	 * scalar ctor
+	 */
+	Dcm(float c00, float c01, float c02,
+			float c10, float c11, float c12,
+			float c20, float c21, float c22);
+
+	/**
 	 * data ctor
 	 */
 	Dcm(const float *data);
diff --git a/apps/mathlib/math/EulerAngles.cpp b/apps/mathlib/math/EulerAngles.cpp
index 8991d5623..27ebbf8b3 100644
--- a/apps/mathlib/math/EulerAngles.cpp
+++ b/apps/mathlib/math/EulerAngles.cpp
@@ -97,23 +97,27 @@ EulerAngles::~EulerAngles()
 int __EXPORT eulerAnglesTest()
 {
 	printf("Test EulerAngles\t: ");
-	EulerAngles euler(1, 2, 3);
+	EulerAngles euler(0.1, 0.2, 0.3);
 
 	// test ctor
-	ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
-	ASSERT(equal(euler.getPhi(), 1));
-	ASSERT(equal(euler.getTheta(), 2));
-	ASSERT(equal(euler.getPsi(), 3));
+	ASSERT(vectorEqual(Vector3(0.1, 0.2, 0.3), euler));
+	ASSERT(equal(euler.getPhi(), 0.1));
+	ASSERT(equal(euler.getTheta(), 0.2));
+	ASSERT(equal(euler.getPsi(), 0.3));
 
 	// test dcm ctor
+	euler = Dcm(EulerAngles(0.1,0.2,0.3));
+	ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
+
+	// test quat ctor
+	euler = Quaternion(EulerAngles(0.1,0.2,0.3));
+	ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
 
 	// test assignment
-	euler.setPhi(4);
-	ASSERT(equal(euler.getPhi(), 4));
-	euler.setTheta(5);
-	ASSERT(equal(euler.getTheta(), 5));
-	euler.setPsi(6);
-	ASSERT(equal(euler.getPsi(), 6));
+	euler.setPhi(0.4);
+	euler.setTheta(0.5);
+	euler.setPsi(0.6);
+	ASSERT(vectorEqual(Vector3(0.4,0.5,0.6),euler));
 
 	printf("PASS\n");
 	return 0;
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;
 }
diff --git a/apps/mathlib/math/nasa_rotation_def.pdf b/apps/mathlib/math/nasa_rotation_def.pdf
new file mode 100644
index 000000000..eb67a4bfc
--- /dev/null
+++ b/apps/mathlib/math/nasa_rotation_def.pdf
diff --git a/apps/mathlib/math/test_math.sce b/apps/mathlib/math/test_math.sce
new file mode 100644
index 000000000..c3fba4729
--- /dev/null
+++ b/apps/mathlib/math/test_math.sce
@@ -0,0 +1,63 @@
+clc
+clear
+function out = float_truncate(in, digits)
+    out = round(in*10^digits)
+    out = out/10^digits
+endfunction
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+
+cosPhi = cos(phi)
+cosPhi_2 = cos(phi/2)
+sinPhi = sin(phi)
+sinPhi_2 = sin(phi/2)
+
+cosTheta = cos(theta)
+cosTheta_2 = cos(theta/2)
+sinTheta = sin(theta)
+sinTheta_2 = sin(theta/2)
+
+cosPsi = cos(psi)
+cosPsi_2 = cos(psi/2)
+sinPsi = sin(psi)
+sinPsi_2 = sin(psi/2)
+
+C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
+        cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
+        -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
+    
+disp(C_nb)
+//C_nb = float_truncate(C_nb,3)
+//disp(C_nb)
+
+theta = asin(-C_nb(3,1))
+phi = atan(C_nb(3,2), C_nb(3,3))
+psi = atan(C_nb(2,1), C_nb(1,1))
+printf('phi %f\n', phi)
+printf('theta %f\n', theta)
+printf('psi %f\n', psi)
+
+q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
+     sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
+     cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
+     cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
+     
+//q = float_truncate(q,3)
+
+a = q(1)
+b = q(2)
+c = q(3)
+d = q(4)
+printf('q: %f %f %f %f\n', a, b, c, d)
+a2 = a*a
+b2 = b*b
+c2 = c*c
+d2 = d*d
+
+C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
+         2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
+         2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
+
+disp(C2_nb)