From 1cf9f72f628c5dbdf487e464699245cab61c1750 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 3 Nov 2013 12:40:13 -0500 Subject: Add data manager module and fence support to navigator - Add function to geo.c to determine if global position is inside fence - Add navigator support/commands for maintaining fence coords. - Add data manager module to support persistence fence storage. Can store other data, but only used for fence at this time. - Add unit tests for data manager --- src/systemcmds/tests/tests.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src/systemcmds/tests/tests.h') diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c483108cf..4a5eaa0c5 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -101,6 +101,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_dataman(int argc, char *argv[]); __END_DECLS -- cgit v1.2.3 From a83e3cd22276109301678c204e83050483200d6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 19:33:47 +0400 Subject: New mathlib, WIP --- makefiles/config_px4fmu-v1_test.mk | 50 ++++ src/lib/conversion/rotation.cpp | 15 +- src/lib/mathlib/math/Dcm.cpp | 174 ------------- src/lib/mathlib/math/Dcm.hpp | 108 -------- src/lib/mathlib/math/EulerAngles.cpp | 126 --------- src/lib/mathlib/math/EulerAngles.hpp | 74 ------ src/lib/mathlib/math/Matrix.cpp | 193 -------------- src/lib/mathlib/math/Matrix.hpp | 253 ++++++++++++++++-- src/lib/mathlib/math/Matrix3.cpp | 46 ++++ src/lib/mathlib/math/Matrix3.hpp | 348 +++++++++++++++++++++++++ src/lib/mathlib/math/Quaternion.cpp | 174 ------------- src/lib/mathlib/math/Quaternion.hpp | 83 ++---- src/lib/mathlib/math/Vector.cpp | 100 -------- src/lib/mathlib/math/Vector.hpp | 214 ++++++++++++++-- src/lib/mathlib/math/Vector2.hpp | 269 ++++++++++++++++++++ src/lib/mathlib/math/Vector2f.cpp | 103 -------- src/lib/mathlib/math/Vector2f.hpp | 79 ------ src/lib/mathlib/math/Vector3.cpp | 99 -------- src/lib/mathlib/math/Vector3.hpp | 271 +++++++++++++++++--- src/lib/mathlib/math/arm/Matrix.cpp | 40 --- src/lib/mathlib/math/arm/Matrix.hpp | 292 --------------------- src/lib/mathlib/math/arm/Vector.cpp | 40 --- src/lib/mathlib/math/arm/Vector.hpp | 236 ----------------- src/lib/mathlib/math/generic/Matrix.cpp | 40 --- src/lib/mathlib/math/generic/Matrix.hpp | 437 -------------------------------- src/lib/mathlib/math/generic/Vector.cpp | 40 --- src/lib/mathlib/math/generic/Vector.hpp | 245 ------------------ src/lib/mathlib/mathlib.h | 11 +- src/lib/mathlib/module.mk | 18 +- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_mathlib.cpp | 114 +++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 33 files changed, 1528 insertions(+), 2767 deletions(-) create mode 100644 makefiles/config_px4fmu-v1_test.mk delete mode 100644 src/lib/mathlib/math/Dcm.cpp delete mode 100644 src/lib/mathlib/math/Dcm.hpp delete mode 100644 src/lib/mathlib/math/EulerAngles.cpp delete mode 100644 src/lib/mathlib/math/EulerAngles.hpp delete mode 100644 src/lib/mathlib/math/Matrix.cpp create mode 100644 src/lib/mathlib/math/Matrix3.cpp create mode 100644 src/lib/mathlib/math/Matrix3.hpp delete mode 100644 src/lib/mathlib/math/Quaternion.cpp delete mode 100644 src/lib/mathlib/math/Vector.cpp create mode 100644 src/lib/mathlib/math/Vector2.hpp delete mode 100644 src/lib/mathlib/math/Vector2f.cpp delete mode 100644 src/lib/mathlib/math/Vector2f.hpp delete mode 100644 src/lib/mathlib/math/Vector3.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.hpp delete mode 100644 src/lib/mathlib/math/arm/Vector.cpp delete mode 100644 src/lib/mathlib/math/arm/Vector.hpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.cpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.hpp delete mode 100644 src/lib/mathlib/math/generic/Vector.cpp delete mode 100644 src/lib/mathlib/math/generic/Vector.hpp create mode 100644 src/systemcmds/tests/test_mathlib.cpp (limited to 'src/systemcmds/tests/tests.h') diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk new file mode 100644 index 000000000..42a35c5dd --- /dev/null +++ b/makefiles/config_px4fmu-v1_test.mk @@ -0,0 +1,50 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/led +MODULES += drivers/boards/px4fmu-v1 +MODULES += systemcmds/perf +MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/uORB +MODULES += modules/systemlib/mixer + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b078562c2..bac594ab9 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,22 +41,11 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix) { - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - (*rot_matrix)(i, j) = R(i, j); - } - } + rot_matrix->from_euler(roll, pitch, yaw); } diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081..000000000 --- a/src/lib/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.cpp - * - * math direction cosine matrix - */ - -#include - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -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[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - 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.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.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; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - 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; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // 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"); - return 0; -} -} // namespace math diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp deleted file mode 100644 index df8970d3a..000000000 --- a/src/lib/mathlib/math/Dcm.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.hpp - * - * math direction cosine matrix - */ - -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Quaternion; -class EulerAngles; - -/** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. - */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - 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); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math - diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb..000000000 --- a/src/lib/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - 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(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; -} - -} // namespace math diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp deleted file mode 100644 index 399eecfa7..000000000 --- a/src/lib/mathlib/math/EulerAngles.hpp +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class Quaternion; -class Dcm; - -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); - - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } - - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } - -}; - -int __EXPORT eulerAnglesTest(); - -} // math - diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3..000000000 --- a/src/lib/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15ec..f05360a19 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +34,241 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix3.hpp * - * matrix code + * 3x3 Matrix */ -#pragma once +#ifndef MATRIX_HPP +#define MATRIX_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif +#include "../CMSIS/Include/arm_math.h" namespace math { + +template class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + +// MxN matrix with float elements +template +class MatrixBase { +public: + /** + * matrix data[row][col] + */ + float data[M][N]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + MatrixBase() { + //arm_mat_init_f32(&arm_mat, M, N, data); + arm_mat = {M, N, &data[0][0]}; + } + + /** + * access by index + */ + inline float &operator ()(unsigned int row, unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + inline const float &operator ()(unsigned int row, unsigned int col) const { + return data[row][col]; + } + + unsigned int getRows() { + return M; + } + + unsigned int getCols() { + return N; + } + + /** + * test for equality + */ + bool operator ==(const MatrixBase &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const MatrixBase &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return true; + return false; + } + + /** + * set to value + */ + const MatrixBase &operator =(const MatrixBase &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + MatrixBase operator -(void) const { + MatrixBase res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = -data[i][j]; + return res; + } + + /** + * addition + */ + MatrixBase operator +(const MatrixBase &m) const { + MatrixBase res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = data[i][j] + m(i, j); + return res; + } + + MatrixBase &operator +=(const MatrixBase &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + MatrixBase operator -(const MatrixBase &m) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] - m(i, j); + return res; + } + + MatrixBase &operator -=(const MatrixBase &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + MatrixBase operator *(const float num) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] * num; + return res; + } + + MatrixBase &operator *=(const float num) { + return *this = *this * num; + } + + MatrixBase operator /(const float num) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + return res; + } + + MatrixBase &operator /=(const float num) { + return *this = *this / num; + } + + /** + * multiplication by another matrix + */ + template + Matrix operator *(const Matrix &m) const { + Matrix res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; + } + + /** + * setup the identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + for (unsigned int i = 0; i < M; i++) + data[i][i] = 1; + } + + void dump(void) { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) + printf("%.3f\t", data[i][j]); + printf("\n"); + } + } +}; + +template +class Matrix : public MatrixBase { +public: + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + /* + Vector operator *(const Vector &v) const { + Vector res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; + } + */ +}; + +template <> +class Matrix<3, 3> : public MatrixBase<3, 3> { +public: + /** + * set to value + */ + const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + /* + Vector<3> operator *(const Vector<3> &v) const { + Vector<3> res; + res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2); + res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2); + res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2); + return res; + } + */ +}; + +} + +#endif // MATRIX_HPP diff --git a/src/lib/mathlib/math/Matrix3.cpp b/src/lib/mathlib/math/Matrix3.cpp new file mode 100644 index 000000000..3f1259dc4 --- /dev/null +++ b/src/lib/mathlib/math/Matrix3.cpp @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix3.cpp + * + * 3x3 Matrix + */ + +#include "Matrix3.hpp" + +namespace math +{ +} diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp new file mode 100644 index 000000000..3236cd3d1 --- /dev/null +++ b/src/lib/mathlib/math/Matrix3.hpp @@ -0,0 +1,348 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix3.hpp + * + * 3x3 Matrix + */ + +#ifndef MATRIX3_HPP +#define MATRIX3_HPP + +#include "Vector3.hpp" +#include "../CMSIS/Include/arm_math.h" + +namespace math +{ + +// 3x3 matrix with elements of type T +template +class Matrix3 { +public: + /** + * matrix data[row][col] + */ + T data[3][3]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + Matrix3() { + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * setting ctor + */ + Matrix3(const T d[3][3]) { + memcpy(data, d, sizeof(data)); + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * setting ctor + */ + Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { + data[0][0] = ax; + data[0][1] = ay; + data[0][2] = az; + data[1][0] = bx; + data[1][1] = by; + data[1][2] = bz; + data[2][0] = cx; + data[2][1] = cy; + data[2][2] = cz; + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * casting from a vector3f to a matrix is the tilde operator + */ + Matrix3(const Vector3 &v) { + arm_mat = {3, 3, &data[0][0]}; + data[0][0] = 0; + data[0][1] = -v.z; + data[0][2] = v.y; + data[1][0] = v.z; + data[1][1] = 0; + data[1][2] = -v.x; + data[2][0] = -v.y; + data[2][1] = v.x; + data[2][2] = 0; + } + + /** + * access by index + */ + inline T &operator ()(unsigned int row, unsigned int col) { + return data[row][col]; + } + + /** + * access to elements by index + */ + inline const T &operator ()(unsigned int row, unsigned int col) const { + return data[row][col]; + } + + /** + * test for equality + */ + bool operator ==(const Matrix3 &m) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (data[i][j] != m(i, j)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Matrix3 &m) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (data[i][j] != m(i, j)) + return true; + return false; + } + + /** + * negation + */ + Matrix3 operator -(void) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = -data[i][j]; + return res; + } + + /** + * addition + */ + Matrix3 operator +(const Matrix3 &m) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] + m(i, j); + return res; + } + + Matrix3 &operator +=(const Matrix3 &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + Matrix3 operator -(const Matrix3 &m) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] - m(i, j); + return res; + } + + Matrix3 &operator -=(const Matrix3 &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + Matrix3 operator *(const T num) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] * num; + return res; + } + + Matrix3 &operator *=(const T num) { + return *this = *this * num; + } + + Matrix3 operator /(const T num) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] / num; + return res; + } + + Matrix3 &operator /=(const T num) { + return *this = *this / num; + } + + /** + * multiplication by a vector + */ + Vector3 operator *(const Vector3 &v) const { + return Vector3( + data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z, + data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z, + data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z); + } + + /** + * multiplication of transpose by a vector + */ + Vector3 mul_transpose(const Vector3 &v) const { + return Vector3( + data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z, + data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z, + data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z); + } + + /** + * multiplication by another matrix + */ + Matrix3 operator *(const Matrix3 &m) const { +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) + Matrix3 res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; +#else + return Matrix3(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0), + data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1), + data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2), + data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0), + data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1), + data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2), + data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0), + data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1), + data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2)); +#endif + } + + Matrix3 &operator *=(const Matrix3 &m) { + return *this = *this * m; + } + + /** + * transpose the matrix + */ + Matrix3 transposed(void) const { +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float + Matrix3 res; + arm_mat_trans_f32(&arm_mat, &res.arm_mat); + return res; +#else + return Matrix3(data[0][0], data[1][0], data[2][0], + data[0][1], data[1][1], data[2][1], + data[0][2], data[1][2], data[2][2]); +#endif + } + + /** + * inverse the matrix + */ + Matrix3 inversed(void) const { + Matrix3 res; + arm_mat_inverse_f32(&arm_mat, &res.arm_mat); + return res; + } + + /** + * zero the matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * setup the identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + data[0][0] = 1; + data[1][1] = 1; + data[2][2] = 1; + } + + /** + * check if any elements are NAN + */ + bool is_nan(void) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (isnan(data[i][j])) + return true; + return false; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(T roll, T pitch, T yaw) { + T cp = (T)cosf(pitch); + T sp = (T)sinf(pitch); + T sr = (T)sinf(roll); + T cr = (T)cosf(roll); + T sy = (T)sinf(yaw); + T cy = (T)cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + // create eulers from a rotation matrix + //void to_euler(float *roll, float *pitch, float *yaw); + + // apply an additional rotation from a body frame gyro vector + // to a rotation matrix. + //void rotate(const Vector3 &g); +}; + +typedef Matrix3 Matrix3f; +} + +#endif // MATRIX3_HPP diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca6..000000000 --- a/src/lib/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // 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) -{ - 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 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - 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.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.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - 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; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d33..64b980ad8 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,82 +36,35 @@ /** * @file Quaternion.hpp * - * math quaternion lib + * Quaternion */ -#pragma once +#ifndef QUATERNION_HPP +#define QUATERNION_HPP -#include "Vector.hpp" -#include "Matrix.hpp" +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector -{ +class Quaternion { public: + float real; + Vector3 imag; /** - * default ctor - */ - Quaternion(); - - /** - * ctor from floats - */ - Quaternion(float a, float b, float c, float d); - - /** - * ctor from data - */ - Quaternion(const float *data); - - /** - * ctor from Vector - */ - Quaternion(const Vector &v); - - /** - * ctor from EulerAngles + * trivial ctor */ - Quaternion(const EulerAngles &euler); + Quaternion() { + } /** - * ctor from Dcm + * setting ctor */ - Quaternion(const Dcm &dcm); - - /** - * deep copy ctor - */ - Quaternion(const Quaternion &right); - - /** - * dtor - */ - virtual ~Quaternion(); - - /** - * derivative - */ - Vector derivative(const Vector &w); - - /** - * accessors - */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } + Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) { + } }; +} -int __EXPORT quaternionTest(); -} // math - +#endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp deleted file mode 100644 index 35158a396..000000000 --- a/src/lib/mathlib/math/Vector.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector.hpp" - -namespace math -{ - -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; - -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); - -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} - -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d5..adb2293e1 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +34,204 @@ ****************************************************************************/ /** - * @file Vector.h + * @file Vector.hpp * - * math vector + * Generic Vector */ -#pragma once +#ifndef VECTOR_HPP +#define VECTOR_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + +template +class Vector { +public: + float data[N]; + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + */ + Vector() { + arm_col = {N, 1, &data[0]}; + } + + /** + * setting ctor + */ + Vector(const float *d) { + memcpy(data, d, sizeof(data)); + arm_col = {N, 1, &data[0]}; + } + + /** + * access to elements by index + */ + inline float &operator ()(unsigned int i) { + return data[i]; + } + + /** + * access to elements by index + */ + inline const float &operator ()(unsigned int i) const { + return data[i]; + } + + /** + * test for equality + */ + bool operator ==(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return true; + return false; + } + + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(data, v.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + const Vector operator -(void) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = -data[i]; + return res; + } + + /** + * addition + */ + const Vector operator +(const Vector &v) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] + v(i); + return res; + } + + /** + * subtraction + */ + const Vector operator -(const Vector &v) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] - v(i); + return res; + } + + /** + * uniform scaling + */ + const Vector operator *(const float num) const { + Vector temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector operator /(const float num) const { + Vector temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector &operator +=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] += v(i); + return *this; + } + + /** + * subtraction + */ + const Vector &operator -=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] -= v(i); + return *this; + } + + /** + * uniform scaling + */ + const Vector &operator *=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector &operator /=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + return *this; + } + + /** + * dot product + */ + float operator *(const Vector &v) const { + float res; + for (unsigned int i = 0; i < N; i++) + res += data[i] * v(i); + return res; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return sqrtf(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector normalized() const { + return *this / length(); + } +}; + +} + +#endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp new file mode 100644 index 000000000..501740614 --- /dev/null +++ b/src/lib/mathlib/math/Vector2.hpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector2.hpp + * + * 2D Vector + */ + +#ifndef VECTOR2_HPP +#define VECTOR2_HPP + +#include + +namespace math +{ + +template +class Vector2 { +public: + T x, y; + + /** + * trivial ctor + */ + Vector2() { + } + + /** + * setting ctor + */ + Vector2(const T x0, const T y0): x(x0), y(y0) { + } + + /** + * setter + */ + void set(const T x0, const T y0) { + x = x0; + y = y0; + } + + /** + * access to elements by index + */ + T operator ()(unsigned int i) { + return *(&x + i); + } + + /** + * access to elements by index + */ + const T operator ()(unsigned int i) const { + return *(&x + i); + } + + /** + * test for equality + */ + bool operator ==(const Vector2 &v) { + return (x == v.x && y == v.y); + } + + /** + * test for inequality + */ + bool operator !=(const Vector2 &v) { + return (x != v.x || y != v.y); + } + + /** + * set to value + */ + const Vector2 &operator =(const Vector2 &v) { + x = v.x; + y = v.y; + return *this; + } + + /** + * negation + */ + const Vector2 operator -(void) const { + return Vector2(-x, -y); + } + + /** + * addition + */ + const Vector2 operator +(const Vector2 &v) const { + return Vector2(x + v.x, y + v.y); + } + + /** + * subtraction + */ + const Vector2 operator -(const Vector2 &v) const { + return Vector2(x - v.x, y - v.y); + } + + /** + * uniform scaling + */ + const Vector2 operator *(const T num) const { + Vector2 temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector2 operator /(const T num) const { + Vector2 temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector2 &operator +=(const Vector2 &v) { + x += v.x; + y += v.y; + return *this; + } + + /** + * subtraction + */ + const Vector2 &operator -=(const Vector2 &v) { + x -= v.x; + y -= v.y; + return *this; + } + + /** + * uniform scaling + */ + const Vector2 &operator *=(const T num) { + x *= num; + y *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector2 &operator /=(const T num) { + x /= num; + y /= num; + return *this; + } + + /** + * dot product + */ + T operator *(const Vector2 &v) const { + return x * v.x + y * v.y; + } + + /** + * cross product + */ + const float operator %(const Vector2 &v) const { + return x * v.y - y * v.x; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return (T)sqrt(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector2 normalized() const { + return *this / length(); + } + + /** + * reflects this vector about n + */ + void reflect(const Vector2 &n) + { + Vector2 orig(*this); + project(n); + *this = *this * 2 - orig; + } + + /** + * projects this vector onto v + */ + void project(const Vector2 &v) { + *this = v * (*this * v) / (v * v); + } + + /** + * returns this vector projected onto v + */ + Vector2 projected(const Vector2 &v) { + return v * (*this * v) / (v * v); + } + + /** + * computes the angle between 2 arbitrary vectors + */ + static inline float angle(const Vector2 &v1, const Vector2 &v2) { + return acosf((v1 * v2) / (v1.length() * v2.length())); + } + + /** + * computes the angle between 2 arbitrary normalized vectors + */ + static inline float angle_normalized(const Vector2 &v1, const Vector2 &v2) { + return acosf(v1 * v2); + } +}; + +typedef Vector2 Vector2f; +} + +#endif // VECTOR2_HPP diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp deleted file mode 100644 index 68e741817..000000000 --- a/src/lib/mathlib/math/Vector2f.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector2f.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector2f.hpp" - -namespace math -{ - -Vector2f::Vector2f() : - Vector(2) -{ -} - -Vector2f::Vector2f(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} - -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} - -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} - -Vector2f::~Vector2f() -{ -} - -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} - -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} - -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp deleted file mode 100644 index ecd62e81c..000000000 --- a/src/lib/mathlib/math/Vector2f.hpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector2f.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math - diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600e..000000000 --- a/src/lib/mathlib/math/Vector3.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector3.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector3.hpp" - -namespace math -{ - -Vector3::Vector3() : - Vector(3) -{ -} - -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} - -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} - -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} - -Vector3::~Vector3() -{ -} - -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} - -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp index 568d9669a..a0c3b9290 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/lib/mathlib/math/Vector3.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,43 +36,252 @@ /** * @file Vector3.hpp * - * math 3 vector + * 3D Vector */ -#pragma once +#ifndef VECTOR3_HPP +#define VECTOR3_HPP -#include "Vector.hpp" +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class __EXPORT Vector3 : - public Vector -{ +template +class Vector3 { public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ + T x, y, z; + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + */ + Vector3() { + arm_col = {3, 1, &x}; + } + + /** + * setting ctor + */ + Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { + arm_col = {3, 1, &x}; + } + + /** + * setting ctor + */ + Vector3(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { + arm_col = {3, 1, &x}; + } + + /** + * setter + */ + void set(const T x0, const T y0, const T z0) { + x = x0; + y = y0; + z = z0; + } + + /** + * access to elements by index + */ + T operator ()(unsigned int i) { + return *(&x + i); + } + + /** + * access to elements by index + */ + const T operator ()(unsigned int i) const { + return *(&x + i); + } + + /** + * test for equality + */ + bool operator ==(const Vector3 &v) { + return (x == v.x && y == v.y && z == v.z); + } + + /** + * test for inequality + */ + bool operator !=(const Vector3 &v) { + return (x != v.x || y != v.y || z != v.z); + } + + /** + * set to value + */ + const Vector3 &operator =(const Vector3 &v) { + x = v.x; + y = v.y; + z = v.z; + return *this; + } + + /** + * negation + */ + const Vector3 operator -(void) const { + return Vector3(-x, -y, -z); + } + + /** + * addition + */ + const Vector3 operator +(const Vector3 &v) const { + return Vector3(x + v.x, y + v.y, z + v.z); + } + + /** + * subtraction + */ + const Vector3 operator -(const Vector3 &v) const { + return Vector3(x - v.x, y - v.y, z - v.z); + } + + /** + * uniform scaling + */ + const Vector3 operator *(const T num) const { + Vector3 temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector3 operator /(const T num) const { + Vector3 temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector3 &operator +=(const Vector3 &v) { + x += v.x; + y += v.y; + z += v.z; + return *this; + } + + /** + * subtraction + */ + const Vector3 &operator -=(const Vector3 &v) { + x -= v.x; + y -= v.y; + z -= v.z; + return *this; + } + + /** + * uniform scaling + */ + const Vector3 &operator *=(const T num) { + x *= num; + y *= num; + z *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector3 &operator /=(const T num) { + x /= num; + y /= num; + z /= num; + return *this; + } + + /** + * dot product + */ + T operator *(const Vector3 &v) const { + return x * v.x + y * v.y + z * v.z; + } + + /** + * cross product + */ + const Vector3 operator %(const Vector3 &v) const { + Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); + return temp; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return (T)sqrt(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector3 normalized() const { + return *this / length(); + } + + /** + * reflects this vector about n + */ + void reflect(const Vector3 &n) + { + Vector3 orig(*this); + project(n); + *this = *this * 2 - orig; + } + + /** + * projects this vector onto v + */ + void project(const Vector3 &v) { + *this = v * (*this * v) / (v * v); + } + + /** + * returns this vector projected onto v + */ + Vector3 projected(const Vector3 &v) { + return v * (*this * v) / (v * v); + } + + /** + * computes the angle between 2 arbitrary vectors + */ + static inline float angle(const Vector3 &v1, const Vector3 &v2) { + return acosf((v1 * v2) / (v1.length() * v2.length())); + } + + /** + * computes the angle between 2 arbitrary normalized vectors + */ + static inline float angle_normalized(const Vector3 &v1, const Vector3 &v2) { + return acosf(v1 * v2); + } }; -int __EXPORT vector3Test(); -} // math +typedef Vector3 Vector3f; +} +#endif // VECTOR3_HPP diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/lib/mathlib/math/arm/Matrix.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "Matrix.hpp" diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 1945bb02d..000000000 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exponent; - float num = (*this)(i, j); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/lib/mathlib/math/arm/Vector.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 52220fc15..000000000 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exponent; - float num = (*this)(i); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/lib/mathlib/math/generic/Matrix.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "Matrix.hpp" diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447..000000000 --- a/src/lib/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/lib/mathlib/math/generic/Vector.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d..000000000 --- a/src/lib/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc..d8e95b46b 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -41,13 +41,12 @@ #pragma once -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" -#include "math/Matrix.hpp" -#include "math/Quaternion.hpp" #include "math/Vector.hpp" +#include "math/Vector2.hpp" #include "math/Vector3.hpp" -#include "math/Vector2f.hpp" +#include "math/Matrix.hpp" +#include "math/Matrix3.hpp" +#include "math/Quaternion.hpp" #include "math/Limits.hpp" #endif @@ -56,4 +55,4 @@ #include "CMSIS/Include/arm_math.h" -#endif \ No newline at end of file +#endif diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 72bc7db8a..50951c617 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,13 +35,7 @@ # Math library # SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ + math/Matrix3.cpp \ math/Limits.cpp # @@ -49,13 +43,3 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) - -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 5d5fe50d3..20182c5d8 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,7 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_mathlib.cpp \ test_file.c \ tests_main.c \ test_param.c \ diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp new file mode 100644 index 000000000..13e6dad51 --- /dev/null +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_mathlib.cpp + * + * Mathlib test + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +using namespace math; + +const char* formatResult(bool res) { + return res ? "OK" : "ERROR"; +} + +int test_mathlib(int argc, char *argv[]) +{ + warnx("testing mathlib"); + + Matrix3f m; + m.identity(); + Matrix3f m1; + Matrix<3,3> mq; + mq.identity(); + Matrix<3,3> mq1; + m1(0, 0) = 5.0; + Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); + Vector3f v1; + + unsigned int n = 60000; + + hrt_abstime t0, t1; + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + v1 = m * v; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mq1 = mq * mq; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); + mq1.dump(); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.transposed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.inversed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + Matrix<4,4> mn; + mn(0, 0) = 2.0f; + mn(1, 0) = 3.0f; + for (int i = 0; i < mn.getRows(); i++) { + for (int j = 0; j < mn.getCols(); j++) { + printf("%.3f ", mn(i, j)); + } + printf("\n"); + } + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 5cbc5ad88..e2105f8ee 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index cd998dd18..fd026d70e 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -105,6 +105,7 @@ const struct { {"bson", test_bson, 0}, {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; -- cgit v1.2.3 From a7012a54180386b5dc7f86fe6a1564690ecc8240 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Mar 2014 08:34:55 +1100 Subject: tests: added "tests file2" filesystem test this is useful for testing for filesystem corruption on cluster boundaries --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_file2.c | 196 ++++++++++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 199 insertions(+) create mode 100644 src/systemcmds/tests/test_file2.c (limited to 'src/systemcmds/tests/tests.h') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index acf28c35b..622a0faf3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -26,6 +26,7 @@ SRCS = test_adc.c \ test_mixer.cpp \ test_mathlib.cpp \ test_file.c \ + test_file2.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c new file mode 100644 index 000000000..ef555f6c3 --- /dev/null +++ b/src/systemcmds/tests/test_file2.c @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_file2.c + * + * File write test. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define FLAG_FSYNC 1 +#define FLAG_LSEEK 2 + +/* + return a predictable value for any file offset to allow detection of corruption + */ +static uint8_t get_value(uint32_t ofs) +{ + union { + uint32_t ofs; + uint8_t buf[4]; + } u; + u.ofs = ofs; + return u.buf[ofs % 4]; +} + +static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags) +{ + printf("Testing on %s with write_chunk=%u write_size=%u\n", + filename, (unsigned)write_chunk, (unsigned)write_size); + + uint32_t ofs = 0; + int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC); + if (fd == -1) { + perror(filename); + exit(1); + } + + // create a file of size write_size, in write_chunk blocks + uint8_t counter = 0; + while (ofs < write_size) { + uint8_t buffer[write_chunk]; + for (uint16_t j=0; j 0) { + filename = argv[0]; + } + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + test_corruption(filename, write_chunk, write_size, flags); + return 0; +} + diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index ac64ad33d..ad55e1410 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -107,6 +107,7 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_file2(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 73827b7cf..fe22f6177 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -104,6 +104,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"file2", test_file2, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, -- cgit v1.2.3