aboutsummaryrefslogtreecommitdiff
path: root/apps/mathlib
diff options
context:
space:
mode:
Diffstat (limited to 'apps/mathlib')
-rw-r--r--apps/mathlib/CMSIS/Include/arm_math.h4
-rw-r--r--apps/mathlib/CMSIS/Makefile7
-rw-r--r--apps/mathlib/Makefile20
-rw-r--r--apps/mathlib/math/Dcm.cpp91
-rw-r--r--apps/mathlib/math/Dcm.hpp7
-rw-r--r--apps/mathlib/math/EulerAngles.cpp26
-rw-r--r--apps/mathlib/math/Quaternion.cpp75
-rw-r--r--apps/mathlib/math/nasa_rotation_def.pdfbin0 -> 709235 bytes
-rw-r--r--apps/mathlib/math/test_math.sce63
9 files changed, 194 insertions, 99 deletions
diff --git a/apps/mathlib/CMSIS/Include/arm_math.h b/apps/mathlib/CMSIS/Include/arm_math.h
index fabd98be7..f224d3eb0 100644
--- a/apps/mathlib/CMSIS/Include/arm_math.h
+++ b/apps/mathlib/CMSIS/Include/arm_math.h
@@ -5520,7 +5520,7 @@ extern "C"
*pIa = Ialpha;
/* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
- *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
+ *pIb = -0.5f * Ialpha + (float32_t) 0.8660254039f *Ibeta;
}
@@ -5898,7 +5898,7 @@ extern "C"
/* Iniatilize output for below specified range as least output value of table */
y = pYData[0];
}
- else if(i >= S->nValues)
+ else if((unsigned)i >= S->nValues)
{
/* Iniatilize output for above specified range as last output value of table */
y = pYData[S->nValues - 1];
diff --git a/apps/mathlib/CMSIS/Makefile b/apps/mathlib/CMSIS/Makefile
index fa5de668a..9e28518bc 100644
--- a/apps/mathlib/CMSIS/Makefile
+++ b/apps/mathlib/CMSIS/Makefile
@@ -45,6 +45,13 @@ INCLUDES += $(DSPLIB_SRCDIR)/Include \
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM3/Include
+# Suppress some warnings that ARM should fix, but haven't.
+EXTRADEFINES += -Wno-unsuffixed-float-constants \
+ -Wno-sign-compare \
+ -Wno-shadow \
+ -Wno-float-equal \
+ -Wno-unused-variable
+
#
# Override the default visibility for symbols, since the CMSIS DSPLib doesn't
# have anything we can use to mark exported symbols.
diff --git a/apps/mathlib/Makefile b/apps/mathlib/Makefile
index e5fab1e35..7eebd6ae0 100644
--- a/apps/mathlib/Makefile
+++ b/apps/mathlib/Makefile
@@ -31,8 +31,6 @@
#
############################################################################
-include $(TOPDIR)/.config
-
#
# Math library
#
@@ -44,27 +42,21 @@ CXXSRCS = math/test/test.cpp \
math/Dcm.cpp \
math/Matrix.cpp
-CXXHDRS = math/test/test.hpp \
- math/Vector.hpp \
- math/Vector3.hpp \
- math/EulerAngles.hpp \
- math/Quaternion.hpp \
- math/Dcm.hpp \
- math/Matrix.hpp
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE := $(lastword $(MAKEFILE_LIST))
+-include $(TOPDIR)/.config
-# XXX this really should be a CONFIG_* test
ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
INCLUDES += math/arm
CXXSRCS += math/arm/Vector.cpp \
math/arm/Matrix.cpp
-CXXHDRS += math/arm/Vector.hpp \
- math/arm/Matrix.hpp
else
INCLUDES += math/generic
CXXSRCS += math/generic/Vector.cpp \
math/generic/Matrix.cpp
-CXXHDRS += math/generic/Vector.hpp \
- math/generic/Matrix.hpp
endif
include $(APPDIR)/mk/app.mk
diff --git a/apps/mathlib/math/Dcm.cpp b/apps/mathlib/math/Dcm.cpp
index 59f3d88ab..df0f09b20 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,18 +133,30 @@ Dcm::~Dcm()
int __EXPORT dcmTest()
{
printf("Test DCM\t\t: ");
- Vector3 vB(1, 2, 3);
- ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
+ // default ctor
+ ASSERT(matrixEqual(Dcm(),
Matrix::identity(3)));
- ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
- Matrix::identity(3)));
- ASSERT(vectorEqual(Vector3(-2, 1, 3),
- Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
- ASSERT(vectorEqual(Vector3(3, 2, -1),
- Dcm(EulerAngles(0, M_PI_2_F, 0))*vB));
- ASSERT(vectorEqual(Vector3(1, -3, 2),
- Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB));
- ASSERT(vectorEqual(Vector3(3, 2, -1),
+ // quaternion ctor
+ ASSERT(matrixEqual(
+ Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
+ 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,
+ 0.2896295f, 0.9564251f, -0.0369570f,
+ -0.1986693f, 0.0978434f, 0.9751703f)));
+ // rotations
+ Vector3 vB(1, 2, 3);
+ ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
+ Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
+ ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
+ Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
+ ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
+ Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
+ ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
Dcm(EulerAngles(
M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
printf("PASS\n");
diff --git a/apps/mathlib/math/Dcm.hpp b/apps/mathlib/math/Dcm.hpp
index de69a7aa4..28d840b10 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..2e96fef4c 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.1f, 0.2f, 0.3f);
// 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.1f, 0.2f, 0.3f), euler));
+ ASSERT(equal(euler.getPhi(), 0.1f));
+ ASSERT(equal(euler.getTheta(), 0.2f));
+ ASSERT(equal(euler.getPsi(), 0.3f));
// test dcm ctor
+ euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
+ 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));
// 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.4f);
+ euler.setTheta(0.5f);
+ euler.setPsi(0.6f);
+ ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
printf("PASS\n");
return 0;
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;
}
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
Binary files differ
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)
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 09:05:01 +0000