dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

More formatting.

This commit is contained in:
James Goppert 2013-01-13 19:23:59 -05:00
parent e02791ee8e
commit 0ccdbd78f6
4 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
apps/mathlib/math/Dcm.cpp
View File

@ -58,15 +58,15 @@ Dcm::Dcm(float c00, float c01, float c02,
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(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) :
@ -139,13 +139,13 @@ int __EXPORT dcmTest()
// quaternion ctor
ASSERT(matrixEqual(
Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
Dcm( 0.9362934f, -0.2750958f, 0.2183507f,
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
0.2896295f, 0.9564251f, -0.0369570f,
-0.1986693f, 0.0978434f, 0.9751703f)));
// euler angle ctor
ASSERT(matrixEqual(
Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
Dcm( 0.9362934f, -0.2750958f, 0.2183507f,
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
0.2896295f, 0.9564251f, -0.0369570f,
-0.1986693f, 0.0978434f, 0.9751703f)));
// rotations

6
apps/mathlib/math/EulerAngles.cpp
View File

@ -107,17 +107,17 @@ int __EXPORT eulerAnglesTest()
// test dcm ctor
euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f),euler));
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
// test quat ctor
euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f),euler));
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
// test assignment
euler.setPhi(0.4f);
euler.setTheta(0.5f);
euler.setPsi(0.6f);
ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f),euler));
ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
printf("PASS\n");
return 0;

4
apps/mathlib/math/Quaternion.cpp
View File

@ -82,9 +82,9 @@ Quaternion::Quaternion(const Dcm &dcm) :
// avoiding singularities by not using
// division equations
setA(0.5 * sqrt(1.0 +
double( dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
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))));
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 +