fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 15:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Testing cleanup from Daniel Agar

Browse Source
This commit is contained in:
Lorenz Meier
2016-07-29 13:49:14 +02:00
parent 99aa5f49fc
commit 6ab9dc0acf
56 changed files with 1930 additions and 3416 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/tests/CMakeLists.txt
+20 -13
View File
@@ -33,14 +33,26 @@
set(srcs
test_adc.c
test_autodeclination.cpp
test_bson.c
test_float.c
test_conv.cpp
test_file.c
test_file2.c
test_float.cpp
test_gpio.c
test_hott_telemetry.c
test_hrt.c
test_int.c
test_int.cpp
test_jig_voltages.c
test_led.c
test_mathlib.cpp
test_matrix.cpp
test_mixer.cpp
test_mount.c
test_params.c
test_perf.c
test_ppm_loopback.c
test_rc.c
test_sensors.c
test_servo.c
test_sleep.c
@@ -48,16 +60,7 @@ set(srcs
test_uart_console.c
test_uart_loopback.c
test_uart_send.c
test_mixer.cpp
test_mathlib.cpp
test_file.c
test_file2.c
tests_main.c
test_params.c
test_ppm_loopback.c
test_rc.c
test_conv.cpp
test_mount.c
)
if(${OS} STREQUAL "nuttx")
@@ -69,11 +72,15 @@ endif()
px4_add_module(
MODULE systemcmds__tests
MAIN tests
STACK_MAIN 9000
STACK_MAX 9000
STACK_MAIN 10000
STACK_MAX 10000
COMPILE_FLAGS
${MODULE_CFLAGS}
-Wno-unused-result
-Wno-float-equal
-Wno-missing-declarations
-Wno-double-promotion
-Wno-unknown-warning-option
-Os
SRCS ${srcs}
DEPENDS
src/systemcmds/tests/test_autodeclination.cpp
+38
View File
@@ -0,0 +1,38 @@
#include <unit_test/unit_test.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <px4iofirmware/px4io.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
class AutoDeclinationTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool autodeclination_check();
};
bool AutoDeclinationTest::autodeclination_check(void)
{
ut_assert("declination differs more than 1 degree", get_mag_declination(47.0, 8.0) - 0.6f < 0.5f);
return true;
}
bool AutoDeclinationTest::run_tests(void)
{
ut_run_test(autodeclination_check);
return (_tests_failed == 0);
}
ut_declare_test_c(test_autodeclination, AutoDeclinationTest)
src/systemcmds/tests/test_conv.cpp
+2 -2
View File
@@ -59,7 +59,7 @@
int test_conv(int argc, char *argv[])
{
PX4_INFO("Testing system conversions");
//PX4_INFO("Testing system conversions");
for (int i = -10000; i <= 10000; i += 1) {
float f = i / 10000.0f;
@@ -72,7 +72,7 @@ int test_conv(int argc, char *argv[])
}
}
PX4_INFO("All conversions clean");
//PX4_INFO("All conversions clean");
return 0;
}
src/systemcmds/tests/test_eigen.cpp
-564
View File
@@ -1,564 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 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_eigen.cpp
*
* Eigen test (based of mathlib test)
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Nuno Marques <n.marques21@hotmail.com>
*/
#include <cmath>
#include <px4_eigen.h>
#include <mathlib/mathlib.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "tests.h"
using namespace Eigen;
typedef Matrix<float, 10, 1> Vector10f;
static constexpr size_t OPERATOR_ITERATIONS = 30000;
const float min = -M_PI_F;
const float max = M_PI_F;
const float step = M_PI_F / 12;
const float epsilon_f = 1E-4;
uint8_t err_num;
#define TEST_OP(_title, _op) \
{ \
const hrt_abstime t0 = hrt_absolute_time(); \
for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
_op; \
} \
printf("%llu: %s: finished in: %.6fus\n", (unsigned long long)t0, _title, static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
}
#define VERIFY_OP(_title, _op, __OP_TEST__) \
{ \
_op; \
if (!(__OP_TEST__)) { \
printf(_title ": Failed! (" # __OP_TEST__ ")\n"); \
++err_num; \
} \
}
#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \
VERIFY_OP(_title, _op, __OP_TEST__) \
TEST_OP(_title, _op)
#define EXPECT_QUATERNION(q_exp, q_act, epsilon) \
(fabsf(q_exp.x() - q_act.x()) <= epsilon && \
fabsf(q_exp.y() - q_act.y()) <= epsilon && \
fabsf(q_exp.z() - q_act.z()) <= epsilon && \
fabsf(q_exp.w() - q_act.w()) <= epsilon)
#define EXPECT_NEAR(expected, actual, epsilon) \
(fabsf(expected - actual) <= epsilon)
/**
* @brief
* Prints an Eigen::Matrix to stdout
**/
template<typename T>
void printEigen(const Eigen::MatrixBase<T> &b)
{
for (int i = 0; i < b.rows(); ++i) {
printf("(");
for (int j = 0; j < b.cols(); ++j) {
if (j > 0) {
printf(",");
}
printf("%.3f", static_cast<double>(b(i, j)));
}
printf(")%s\n", i + 1 < b.rows() ? "," : "");
}
}
// Methods definition
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy);
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q);
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy);
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q);
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
/**
* @brief
* Construct new Eigen::Quaternion from euler angles
* Right order is YPR.
**/
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy)
{
return Eigen::Quaternionf(
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
);
}
/**
* @brief
* Construct new Eigen::Vector3f of euler angles from quaternion
* Right order is YPR.
**/
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q)
{
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
}
/**
* @brief
* Construct new Eigen::Matrix3f from euler angles
**/
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy)
{
return quatFromEuler(rpy).toRotationMatrix();
}
/**
* @brief
* Adjust PX4 math::quaternion to Eigen::Quaternionf
**/
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q)
{
return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
}
/**
* @brief
* Adjust Eigen::Quaternionf to PX4 math::quaternion
**/
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q)
{
return math::Quaternion(q.w(), q.x(), q.y(), q.z());
}
/**
* @brief
* Testing main routine
**/
int test_eigen(int argc, char *argv[])
{
int rc = 0;
warnx("Testing Eigen math...");
{
Eigen::Vector2f v;
Eigen::Vector2f v1(1.0f, 2.0f);
Eigen::Vector2f v2(1.0f, -1.0f);
float data[2] = {1.0f, 2.0f};
TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
}
{
Eigen::Vector3f v;
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
float data[3] = {1.0f, 2.0f, 3.0f};
TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
TEST_OP("Vector3f = Vector3f", v = v1);
TEST_OP("Vector3f + Vector3f", v + v1);
TEST_OP("Vector3f - Vector3f", v - v1);
TEST_OP("Vector3f += Vector3f", v += v1);
TEST_OP("Vector3f -= Vector3f", v -= v1);
TEST_OP("Vector3f * float", v1 * 2.0f);
TEST_OP("Vector3f / float", v1 / 2.0f);
TEST_OP("Vector3f *= float", v1 *= 2.0f);
TEST_OP("Vector3f /= float", v1 /= 2.0f);
TEST_OP("Vector3f dot Vector3f", v.dot(v1));
TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
TEST_OP("Vector3f length", v1.norm());
TEST_OP("Vector3f length squared", v1.squaredNorm());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
// Need pragma here instead of moving variable out of TEST_OP and just reference because
// TEST_OP measures performance of vector operations.
TEST_OP("Vector<3> element read", volatile float a = v1(0));
TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
#pragma GCC diagnostic pop
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
}
{
Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
Eigen::Vector4f vres;
float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
TEST_OP("Vector<4> = Vector<4>", v = v1);
TEST_OP("Vector<4> + Vector<4>", v + v1);
TEST_OP("Vector<4> - Vector<4>", v - v1);
TEST_OP("Vector<4> += Vector<4>", v += v1);
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
}
{
Vector10f v1;
v1.Zero();
float data[10];
TEST_OP("Constructor Vector<10>()", Vector10f v3);
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector10f v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Vector10f v3(data));
}
{
Eigen::Matrix3f m1;
m1.setIdentity();
Eigen::Matrix3f m2;
m2.setIdentity();
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
TEST_OP("Matrix3f * Vector3f", m1 * v1);
TEST_OP("Matrix3f + Matrix3f", m1 + m2);
TEST_OP("Matrix3f * Matrix3f", m1 * m2);
}
{
Eigen::Matrix<float, 10, 10> m1;
m1.setIdentity();
Eigen::Matrix<float, 10, 10> m2;
m2.setIdentity();
Vector10f v1;
v1.Zero();
TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
}
{
printf("%llu: Nonsymmetric matrix operations test...\n", (unsigned long long)hrt_absolute_time());
// test nonsymmetric +, -, +=, -=
const Eigen::Matrix<float, 2, 3> m1_orig =
(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
4, 5, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
Eigen::Matrix<float, 2, 3> m2;
m2 << 2, 4, 6,
8, 10, 12;
Eigen::Matrix<float, 2, 3> m3;
m3 << 3, 6, 9,
12, 15, 18;
if (m1 + m2 != m3) {
printf("%llu: Matrix<2, 3> + Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1 + m2);
printf("!=\n");
printEigen(m3);
++err_num;
rc = 1;
}
if (m3 - m2 != m1) {
printf("%llu: Matrix<2, 3> - Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m3 - m2);
printf("!=\n");
printEigen(m1);
++err_num;
rc = 1;
}
m1 += m2;
if (m1 != m3) {
printf("%llu: Matrix<2, 3> += Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1);
printf("!=\n");
printEigen(m3);
++err_num;
rc = 1;
}
m1 -= m2;
if (m1 != m1_orig) {
printf("%llu: Matrix<2, 3> -= Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1);
printf("!=\n");
printEigen(m1_orig);
++err_num;
rc = 1;
}
}
/* QUATERNION TESTS */
{
// Test conversion rotation matrix to quaternion and back
// against existing PX4 mathlib
math::Matrix<3, 3> R_orig;
Eigen::Quaternionf q_true;
Eigen::Quaternionf q;
Eigen::Matrix3f R;
printf("%llu: Conversion method: Quaternion transformation methods test...\n", (unsigned long long)hrt_absolute_time());
printf("%llu: Conversion method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
/******************************************** TEST 1 ****************************************************/
q_true = {0.0f, 0.0f, 0.0f, 1.0f};
math::Quaternion q_px4 = {1.0f, 0.0f, 0.0f, 0.0f};
Eigen::Quaternionf q_eigen(eigenqFromPx4q(q_px4));
if (!EXPECT_QUATERNION(q_true, q_eigen, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion1 [1.0, 0.0, 0.0, 0.0]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q_true = {1.0f, 0.0f, 0.0f, 0.0f};
Eigen::Quaternionf q2_eigen = {0.0f, 0.0f, 0.0f, 1.0f};
math::Quaternion q2_px4(px4qFromEigenq(q2_eigen));
Eigen::Quaternionf q2_eigen_(q2_px4.data[3], q2_px4.data[0], q2_px4.data[1], q2_px4.data[2]);
if (!EXPECT_QUATERNION(q_true, q2_eigen_, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion2 [0.0, 0.0, 0.0, 1.0]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q2_px4.data[0], q2_px4.data[1], q2_px4.data[2], q2_px4.data[3]);
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
q = {0.9833474432563558f, 0.14357217502739184f, 0.10602051106179561f, 0.0342707985504821f};
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion [0.9833, 0.1436, 0.1060, 0.0343]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
q = {0.7222365948153096f, -0.6390766544101811f, -0.2385737751841646f, 0.11418355701173476f};
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion [0.7222, -0.6391, -0.2386, 0.1142]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
q = {0.6830127018922193f, 0.18301270189221933f, -0.6830127018922193f, 0.18301270189221933f};
for (size_t i = 0; i < 4; i++) {
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n",
(unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 6 ****************************************************/
printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
for (float roll = min; roll <= max; roll += step) {
for (float pitch = min; pitch <= max; pitch += step) {
for (float yaw = min; yaw <= max; yaw += step) {
q = Eigen::Quaternionf(quatFromEuler(Eigen::Vector3f(roll, pitch, yaw)));
R = q.toRotationMatrix();
R_orig.from_euler(roll, pitch, yaw);
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n",
(unsigned long long)hrt_absolute_time(), (int)i, (int)j);
printf("Actual: \t%8.5f\n", R(i, j));
printf("Expected: \t%8.5f\n", R_orig(i, j));
++err_num;
rc = 1;
}
}
}
}
}
}
}
{
// Test rotation method (rotate vector by quaternion)
Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
Eigen::Vector3f vector_q;
Eigen::Vector3f vector_r;
Eigen::Quaternionf q;
Eigen::Matrix3f R;
printf("%llu: Rotation method: Quaternion vector rotation method test...\n", (unsigned long long)hrt_absolute_time());
printf("%llu: Rotation method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
/******************************************** TEST 1 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F));
vector_q = q._transformVector(vector);
Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", vector_q(0), vector_q(1), vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", vector_true(0), vector_true(1), vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
vector_q = q._transformVector(vector);
vector_true = {0.8795481794122900f, 1.2090975499501229f, 0.874344391414010f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
vector_q = q._transformVector(vector);
vector_true = {0.447416342848463f, -0.6805264343934600f, 1.528627615949624f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
vector_q = q._transformVector(vector);
vector_true = { -1.366030f, 0.366025f, 1.000000f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
Eigen::Vector3f vectorR(1.0f, 1.0f, 1.0f);
for (float roll = min; roll <= max; roll += step) {
for (float pitch = min; pitch <= max; pitch += step) {
for (float yaw = min; yaw <= max; yaw += step) {
R = matrixFromEuler(Eigen::Vector3f(roll, pitch, yaw));
q = quatFromEuler(Eigen::Vector3f(roll, pitch, yaw));
vector_r = R * vectorR;
vector_q = q._transformVector(vectorR);
for (int i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_r(i), vector_q(i), epsilon_f)) {
printf("%llu: (%d) Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), i);
printf("Actual: \t%8.5f\n", vector_q(i));
printf("Expected: \t%8.5f\n", vector_r(i));
++err_num;
rc = 1;
}
}
}
}
}
}
printf("%llu: Finished Eigen math tests with %d error(s)...\n", (unsigned long long)hrt_absolute_time(), err_num);
return rc;
}
src/systemcmds/tests/test_float.c
-287
View File
@@ -1,287 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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 tests_float.c
* Floating point tests
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include "tests.h"
#include <math.h>
#include <float.h>
typedef union {
float f;
double d;
uint8_t b[8];
} test_float_double_t;
int test_float(int argc, char *argv[])
{
int ret = 0;
printf("\n--- SINGLE PRECISION TESTS ---\n");
printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");
float f1 = 1.55f;
float sinf_zero = sinf(0.0f);
float sinf_one = sinf(1.0f);
float sqrt_two = sqrt(2.0f);
if (fabsf(sinf_zero) < FLT_EPSILON) {
printf("\t success: sinf(0.0f) == 0.0f\n");
} else {
printf("\t FAIL: sinf(0.0f) != 0.0f, result: %8.4f\n", (double)sinf_zero);
ret = -4;
}
fflush(stdout);
if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
printf("\t success: sinf(1.0f) == 0.84147f\n");
} else {
printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %8.4f\n", (double)sinf_one);
ret = -1;
}
fflush(stdout);
float asinf_one = asinf(1.0f);
if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
printf("\t success: asinf(1.0f) == 1.57079f\n");
} else {
printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
ret = -1;
}
fflush(stdout);
float cosf_one = cosf(1.0f);
if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
printf("\t success: cosf(1.0f) == 0.54030f\n");
} else {
printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %8.4f\n", (double)cosf_one);
ret = -1;
}
fflush(stdout);
float acosf_one = acosf(1.0f);
if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
printf("\t success: acosf(1.0f) == 0.0f\n");
} else {
printf("\t FAIL: acosf(1.0f) != 0.0f, result: %8.4f\n", (double)acosf_one);
ret = -1;
}
fflush(stdout);
float sinf_zero_one = sinf(0.1f);
if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
printf("\t success: sinf(0.1f) == 0.09983f\n");
} else {
printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %8.4f\n", (double)sinf_zero_one);
ret = -2;
}
if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
printf("\t success: sqrt(2.0f) == 1.41421f\n");
} else {
printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %8.4f\n", (double)sinf_zero_one);
ret = -3;
}
float atan2f_ones = atan2f(1.0f, 1.0f);
if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON) {
printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");
} else {
printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %8.4f\n", (double)atan2f_ones);
ret = -4;
}
char sbuf[30];
sprintf(sbuf, "%8.4f", (double)0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
} else {
printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \" 0.5534\", result: %s\n", sbuf);
ret = -5;
}
sprintf(sbuf, "%8.4f", (double) - 0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double) - 0.553415f);
} else {
printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
ret = -6;
}
printf("\n--- DOUBLE PRECISION TESTS ---\n");
double d1 = 1.0111;
double d2 = 2.0;
double d1d2 = d1 * d2;
if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
printf("\t success: 1.0111 * 2.0 == 2.0222\n");
} else {
printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %8.4f\n", d1d2);
ret = -7;
}
fflush(stdout);
// Assign value of f1 to d1
d1 = f1;
if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
printf("\t success: (float) 1.55f == 1.55 (double)\n");
} else {
printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
ret = -8;
}
fflush(stdout);
double sin_zero = sin(0.0);
double sin_one = sin(1.0);
double atan2_ones = atan2(1.0, 1.0);
if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
printf("\t success: sin(0.0) == 0.0\n");
} else {
printf("\t FAIL: sin(0.0) != 0.0, result: %8.4f\n", sin_zero);
ret = -9;
}
if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
printf("\t success: sin(1.0) == 0.84147098480\n");
} else {
printf("\t FAIL: sin(1.0) != 1.0, result: %8.4f\n", sin_one);
ret = -10;
}
if (fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON) {
printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
} else {
printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %8.4f\n", atan2_ones);
ret = -11;
}
printf("\t testing pow() with magic value\n");
printf("\t (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
fflush(stdout);
usleep(20000);
double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
printf("\t success: result: %8.4f\n", (double)powres);
sprintf(sbuf, "%8.4f", 0.553415);
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
} else {
printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \" 0.5534\", result: %s\n", sbuf);
ret = -12;
}
sprintf(sbuf, "%8.4f", -0.553415);
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
} else {
printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
ret = -13;
}
if (ret == 0) {
printf("\n SUCCESS: All float and double tests passed.\n");
} else {
printf("\n FAIL: One or more tests failed.\n");
}
printf("\n");
return ret;
}
src/systemcmds/tests/test_float.cpp
+149
View File
@@ -0,0 +1,149 @@
#include <unit_test/unit_test.h>
#include <errno.h>
#include <fcntl.h>
#include <float.h>
#include <math.h>
#include <px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
typedef union {
float f;
double d;
uint8_t b[8];
} test_float_double_t;
class FloatTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool singlePrecisionTests();
bool doublePrecisionTests();
};
bool FloatTest::singlePrecisionTests(void)
{
float sinf_zero = sinf(0.0f);
float sinf_one = sinf(1.0f);
float sqrt_two = sqrt(2.0f);
ut_assert("sinf(0.0f) == 0.0f", fabsf(sinf_zero) < FLT_EPSILON);
ut_assert("sinf(1.0f) == 0.84147f", fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON);
float asinf_one = asinf(1.0f);
ut_assert("asinf(1.0f) == 1.57079f", fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f);
float cosf_one = cosf(1.0f);
ut_assert("cosf(1.0f) == 0.54030f", fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON);
float acosf_one = acosf(1.0f);
ut_assert("acosf(1.0f) == 0.0f", fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON);
float sinf_zero_one = sinf(0.1f);
ut_assert("sinf(0.1f) == 0.09983f", fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON);
ut_assert("sqrt(2.0f) == 1.41421f", fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON);
float atan2f_ones = atan2f(1.0f, 1.0f);
ut_assert("atan2f(1.0f, 1.0f) == 0.78539f",
fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON);
char sbuf[30];
sprintf(sbuf, "%8.4f", (double)0.553415f);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], ' ');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
sprintf(sbuf, "%8.4f", (double) - 0.553415f);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], '-');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
return true;
}
bool FloatTest::doublePrecisionTests(void)
{
float f1 = 1.55f;
double d1 = 1.0111;
double d2 = 2.0;
double d1d2 = d1 * d2;
ut_assert("1.0111 * 2.0 == 2.0222", fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON);
// Assign value of f1 to d1
d1 = f1;
ut_assert("(float) 1.55f == 1.55 (double)", fabsf(f1 - (float)d1) < FLT_EPSILON);
double sin_zero = sin(0.0);
double sin_one = sin(1.0);
double atan2_ones = atan2(1.0, 1.0);
ut_assert("sin(0.0) == 0.0", fabs(sin_zero - 0.0) < DBL_EPSILON);
ut_assert("sin(1.0) == 0.84147098480", fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON);
ut_assert("atan2(1.0, 1.0) == 0.785398", fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON);
ut_assert("testing pow() with magic value",
(44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295))) - 428.2293 < DBL_EPSILON);
char sbuf[30];
sprintf(sbuf, "%8.4f", 0.553415);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], ' ');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
sprintf(sbuf, "%8.4f", -0.553415);
ut_compare("sbuf[0]", sbuf[0], ' ');
ut_compare("sbuf[1]", sbuf[1], '-');
ut_compare("sbuf[2]", sbuf[2], '0');
ut_compare("sbuf[3]", sbuf[3], '.');
ut_compare("sbuf[4]", sbuf[4], '5');
ut_compare("sbuf[5]", sbuf[5], '5');
ut_compare("sbuf[6]", sbuf[6], '3');
ut_compare("sbuf[7]", sbuf[7], '4');
ut_compare("sbuf[8]", sbuf[8], '\0');
return true;
}
bool FloatTest::run_tests(void)
{
ut_run_test(singlePrecisionTests);
ut_run_test(doublePrecisionTests);
return (_tests_failed == 0);
}
ut_declare_test_c(test_float, FloatTest)
src/systemcmds/tests/test_int.c
-152
View File
@@ -1,152 +0,0 @@
/****************************************************************************
* px4/sensors/test_gpio.c
*
* 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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <inttypes.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <arch/board/board.h>
#include "tests.h"
#include <math.h>
#include <float.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_led
****************************************************************************/
typedef union {
int32_t i;
int64_t l;
uint8_t b[8];
} test_32_64_t;
int test_int(int argc, char *argv[])
{
int ret = 0;
printf("\n--- 64 BIT MATH TESTS ---\n");
int64_t large = 354156329598;
int64_t calc = large * 5;
if (calc == 1770781647990) {
printf("\t success: 354156329598 * 5 == %" PRId64 "\n", calc);
} else {
printf("\t FAIL: 354156329598 * 5 != %" PRId64 "\n", calc);
ret = -1;
}
fflush(stdout);
printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n");
int32_t small = 50;
int32_t large_int = 2147483647; // MAX INT value
uint64_t small_times_large = large_int * (uint64_t)small;
if (small_times_large == 107374182350) {
printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %" PRId64 "\n", small_times_large);
} else {
printf("\t FAIL: 50 * 2147483647 != %" PRId64 ", 64bit cast might fail\n", small_times_large);
ret = -1;
}
fflush(stdout);
if (ret == 0) {
printf("\n SUCCESS: All float and double tests passed.\n");
} else {
printf("\n FAIL: One or more tests failed.\n");
}
printf("\n");
return ret;
}
src/systemcmds/tests/test_int.cpp
+61
View File
@@ -0,0 +1,61 @@
#include <unit_test/unit_test.h>
#include <errno.h>
#include <fcntl.h>
#include <float.h>
#include <math.h>
#include <px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
typedef union {
int32_t i;
int64_t l;
uint8_t b[8];
} test_32_64_t;
class IntTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool math64bitTests();
bool math3264MixedMathTests();
};
bool IntTest::math64bitTests(void)
{
int64_t large = 354156329598;
int64_t calc = large * 5;
ut_assert("354156329598 * 5 == 1770781647990", calc == 1770781647990);
return true;
}
bool IntTest::math3264MixedMathTests(void)
{
int32_t small = 50;
int32_t large_int = 2147483647; // MAX INT value
uint64_t small_times_large = large_int * (uint64_t)small;
ut_assert("64bit calculation: 50 * 2147483647 (max int val) == 107374182350", small_times_large == 107374182350);
return true;
}
bool IntTest::run_tests(void)
{
ut_run_test(math64bitTests);
ut_run_test(math3264MixedMathTests);
return (_tests_failed == 0);
}
ut_declare_test_c(test_int, IntTest)
src/systemcmds/tests/test_mathlib.cpp
+240 -164
View File
@@ -31,11 +31,18 @@
*
****************************************************************************/
/**
* @file test_mathlib.cpp
*
* Mathlib test
*/
#include <unit_test/unit_test.h>
#include <errno.h>
#include <fcntl.h>
#include <float.h>
#include <math.h>
#include <px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <px4_log.h>
#include <stdio.h>
@@ -47,17 +54,33 @@
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
class MathlibTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool testVector2();
bool testVector3();
bool testVector4();
bool testVector10();
bool testMatrix3x3();
bool testMatrix10x10();
bool testMatrixNonsymmetric();
bool testRotationMatrixQuaternion();
bool testQuaternionfrom_dcm();
bool testQuaternionfrom_euler();
bool testQuaternionRotate();
};
#include "tests.h"
#define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); }
using namespace math;
int test_mathlib(int argc, char *argv[])
bool MathlibTest::testVector2(void)
{
int rc = 0;
PX4_INFO("testing mathlib");
{
Vector<2> v;
Vector<2> v1(1.0f, 2.0f);
@@ -75,6 +98,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<2> * Vector<2>", v * v1);
TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
}
return true;
}
bool MathlibTest::testVector3(void)
{
{
Vector<3> v;
@@ -108,7 +136,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
}
return true;
}
bool MathlibTest::testVector4(void)
{
{
Vector<4> v;
Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
@@ -125,7 +157,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> * Vector<4>", v * v1);
}
return true;
}
bool MathlibTest::testVector10(void)
{
{
Vector<10> v1;
v1.zero();
@@ -134,7 +170,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
}
return true;
}
bool MathlibTest::testMatrix3x3(void)
{
{
Matrix<3, 3> m1;
m1.identity();
@@ -145,7 +185,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
}
return true;
}
bool MathlibTest::testMatrix10x10(void)
{
{
Matrix<10, 10> m1;
m1.identity();
@@ -157,9 +201,14 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
}
return true;
}
bool MathlibTest::testMatrixNonsymmetric(void)
{
int rc = true;
{
PX4_INFO("Nonsymmetric matrix operations test");
//PX4_INFO("Nonsymmetric matrix operations test");
// test nonsymmetric +, -, +=, -=
float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
@@ -175,17 +224,21 @@ int test_mathlib(int argc, char *argv[])
(m1 + m2).print();
printf("!=\n");
m3.print();
rc = 1;
rc = false;
}
ut_assert("m1 + m2 == m3", m1 + m2 == m3);
if (m3 - m2 != m1) {
PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
(m3 - m2).print();
printf("!=\n");
m1.print();
rc = 1;
rc = false;
}
ut_assert("m3 - m2 == m1", m3 - m2 == m1);
m1 += m2;
if (m1 != m3) {
@@ -193,9 +246,11 @@ int test_mathlib(int argc, char *argv[])
m1.print();
printf("!=\n");
m3.print();
rc = 1;
rc = false;
}
ut_assert("m1 == m3", m1 == m3);
m1 -= m2;
Matrix<2, 3> m1_orig(data1);
@@ -204,160 +259,181 @@ int test_mathlib(int argc, char *argv[])
m1.print();
printf("!=\n");
m1_orig.print();
rc = 1;
rc = false;
}
}
ut_assert("m1 == m1_orig", m1 == m1_orig);
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
float diff = 0.1f;
float tol = 0.00001f;
PX4_INFO("Quaternion transformation methods test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) {
PX4_WARN("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!");
rc = 1;
}
}
}
}
}
}
// test against some known values
tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
R_orig.identity();
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_dcm()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
}
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
float diff = 0.1f;
float tol = 0.00001f;
PX4_INFO("Quaternion vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
return rc;
}
bool MathlibTest::testRotationMatrixQuaternion(void)
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("Quaternion transformation methods test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol);
}
}
}
}
}
return true;
}
bool MathlibTest::testQuaternionfrom_dcm(void)
{
// test against some known values
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
R_orig.identity();
math::Quaternion q;
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
return true;
}
bool MathlibTest::testQuaternionfrom_euler(void)
{
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
R_orig.identity();
math::Quaternion q;
q.from_dcm(R_orig);
q_true.from_euler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
return true;
}
bool MathlibTest::testQuaternionRotate(void)
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("Quaternion vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
return true;
}
bool MathlibTest::run_tests(void)
{
ut_run_test(testVector2);
ut_run_test(testVector3);
ut_run_test(testVector4);
ut_run_test(testVector10);
ut_run_test(testMatrix3x3);
ut_run_test(testMatrix10x10);
ut_run_test(testMatrixNonsymmetric);
ut_run_test(testRotationMatrixQuaternion);
ut_run_test(testQuaternionfrom_dcm);
ut_run_test(testQuaternionfrom_euler);
ut_run_test(testQuaternionRotate);
return (_tests_failed == 0);
}
ut_declare_test_c(test_mathlib, MathlibTest)
src/systemcmds/tests/test_matrix.cpp
+679
View File
@@ -0,0 +1,679 @@
#include <unit_test/unit_test.h>
#include <matrix/math.hpp>
#include <matrix/filter.hpp>
#include <matrix/integration.hpp>
using namespace matrix;
class MatrixTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool attitudeTests();
bool filterTests();
bool helperTests();
bool integrationTests();
bool inverseTests();
bool matrixAssignmentTests();
bool matrixMultTests();
bool matrixScalarMultTests();
bool setIdentityTests();
bool sliceTests();
bool squareMatrixTests();
bool transposeTests();
bool vectorTests();
bool vector2Tests();
bool vector3Tests();
bool vectorAssignmentTests();
};
bool MatrixTest::run_tests(void)
{
ut_run_test(attitudeTests);
ut_run_test(filterTests);
ut_run_test(helperTests);
ut_run_test(integrationTests);
ut_run_test(inverseTests);
ut_run_test(matrixAssignmentTests);
ut_run_test(matrixMultTests);
ut_run_test(matrixScalarMultTests);
ut_run_test(setIdentityTests);
ut_run_test(sliceTests);
ut_run_test(squareMatrixTests);
ut_run_test(transposeTests);
ut_run_test(vectorTests);
ut_run_test(vector2Tests);
ut_run_test(vector3Tests);
ut_run_test(vectorAssignmentTests);
return (_tests_failed == 0);
}
ut_declare_test_c(test_matrix, MatrixTest)
template class matrix::Quaternion<float>;
template class matrix::Euler<float>;
template class matrix::Dcm<float>;
bool MatrixTest::attitudeTests(void)
{
double eps = 1e-6;
// check data
Eulerf euler_check(0.1f, 0.2f, 0.3f);
Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
float dcm_data[] = {
0.93629336f, -0.27509585f, 0.21835066f,
0.28962948f, 0.95642509f, -0.03695701f,
-0.19866933f, 0.0978434f, 0.97517033f
};
Dcmf dcm_check(dcm_data);
// euler ctor
ut_test(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f)));
// euler default ctor
Eulerf e;
Eulerf e_zero = zeros<float, 3, 1>();
ut_test(isEqual(e, e_zero));
ut_test(isEqual(e, e));
// euler vector ctor
Vector<float, 3> v;
v(0) = 0.1f;
v(1) = 0.2f;
v(2) = 0.3f;
Eulerf euler_copy(v);
ut_test(isEqual(euler_copy, euler_check));
// quaternion ctor
Quatf q0(1, 2, 3, 4);
Quatf q(q0);
ut_test(fabs(q(0) - 1) < eps);
ut_test(fabs(q(1) - 2) < eps);
ut_test(fabs(q(2) - 3) < eps);
ut_test(fabs(q(3) - 4) < eps);
// quat normalization
q.normalize();
ut_test(isEqual(q, Quatf(0.18257419f, 0.36514837f,
0.54772256f, 0.73029674f)));
ut_test(isEqual(q0.unit(), q));
// quat default ctor
q = Quatf();
ut_test(isEqual(q, Quatf(1, 0, 0, 0)));
// euler to quaternion
q = Quatf(euler_check);
ut_test(isEqual(q, q_check));
// euler to dcm
Dcmf dcm(euler_check);
ut_test(isEqual(dcm, dcm_check));
// quaternion to euler
Eulerf e1(q_check);
ut_test(isEqual(e1, euler_check));
// quaternion to dcm
Dcmf dcm1(q_check);
ut_test(isEqual(dcm1, dcm_check));
// dcm default ctor
Dcmf dcm2;
SquareMatrix<float, 3> I = eye<float, 3>();
ut_test(isEqual(dcm2, I));
// dcm to euler
Eulerf e2(dcm_check);
ut_test(isEqual(e2, euler_check));
// dcm to quaterion
Quatf q2(dcm_check);
ut_test(isEqual(q2, q_check));
// constants
double deg2rad = M_PI / 180.0;
double rad2deg = 180.0 / M_PI;
// euler dcm round trip check
for (int roll = -90; roll <= 90; roll += 90) {
for (int pitch = -90; pitch <= 90; pitch += 90) {
for (int yaw = -179; yaw <= 180; yaw += 90) {
// note if theta = pi/2, then roll is set to zero
int roll_expected = roll;
int yaw_expected = yaw;
if (pitch == 90) {
roll_expected = 0;
yaw_expected = yaw - roll;
} else if (pitch == -90) {
roll_expected = 0;
yaw_expected = yaw + roll;
}
if (yaw_expected < -180) { yaw_expected += 360; }
if (yaw_expected > 180) { yaw_expected -= 360; }
//printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
Euler<double> euler_expected(
deg2rad * double(roll_expected),
deg2rad * double(pitch),
deg2rad * double(yaw_expected));
Euler<double> euler(
deg2rad * double(roll),
deg2rad * double(pitch),
deg2rad * double(yaw));
Dcm<double> dcm_from_euler(euler);
//dcm_from_euler.print();
Euler<double> euler_out(dcm_from_euler);
ut_test(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
Eulerf eulerf_expected(
float(deg2rad)*float(roll_expected),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw_expected));
Eulerf eulerf(float(deg2rad)*float(roll),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw));
Dcm<float> dcm_from_eulerf(eulerf);
Euler<float> euler_outf(dcm_from_eulerf);
ut_test(isEqual(float(rad2deg)*eulerf_expected,
float(rad2deg)*euler_outf));
}
}
}
// quaterion copy ctors
float data_v4[] = {1, 2, 3, 4};
Vector<float, 4> v4(data_v4);
Quatf q_from_v(v4);
ut_test(isEqual(q_from_v, v4));
Matrix<float, 4, 1> m4(data_v4);
Quatf q_from_m(m4);
ut_test(isEqual(q_from_m, m4));
// quaternion derivate
Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
// quaternion product
Quatf q_prod_check(
0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
ut_test(isEqual(q_prod_check, q_check * q_check));
q_check *= q_check;
ut_test(isEqual(q_prod_check, q_check));
// Quaternion scalar multiplication
float scalar = 0.5;
Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);
Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
3.0f * scalar, 4.0f * scalar);
Quatf q_scalar_mul_res = scalar * q_scalar_mul;
ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res));
Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res2));
Quatf q_scalar_mul_res3(q_scalar_mul);
q_scalar_mul_res3 *= scalar;
ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res3));
// quaternion inverse
q = q_check.inversed();
ut_test(fabs(q_check(0) - q(0)) < eps);
ut_test(fabs(q_check(1) + q(1)) < eps);
ut_test(fabs(q_check(2) + q(2)) < eps);
ut_test(fabs(q_check(3) + q(3)) < eps);
q = q_check;
q.invert();
ut_test(fabs(q_check(0) - q(0)) < eps);
ut_test(fabs(q_check(1) + q(1)) < eps);
ut_test(fabs(q_check(2) + q(2)) < eps);
ut_test(fabs(q_check(3) + q(3)) < eps);
// rotate quaternion (nonzero rotation)
Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
Vector<float, 3> rot;
rot(0) = 1.0f;
rot(1) = rot(2) = 0.0f;
qI.rotate(rot);
Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f);
ut_test(fabs(qI(0) - q_true(0)) < eps);
ut_test(fabs(qI(1) - q_true(1)) < eps);
ut_test(fabs(qI(2) - q_true(2)) < eps);
ut_test(fabs(qI(3) - q_true(3)) < eps);
// rotate quaternion (zero rotation)
qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
rot(0) = 0.0f;
rot(1) = rot(2) = 0.0f;
qI.rotate(rot);
q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f);
ut_test(fabs(qI(0) - q_true(0)) < eps);
ut_test(fabs(qI(1) - q_true(1)) < eps);
ut_test(fabs(qI(2) - q_true(2)) < eps);
ut_test(fabs(qI(3) - q_true(3)) < eps);
// get rotation axis from quaternion (nonzero rotation)
q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
rot = q.to_axis_angle();
ut_test(fabs(rot(0)) < eps);
ut_test(fabs(rot(1) - 1.0f) < eps);
ut_test(fabs(rot(2)) < eps);
// get rotation axis from quaternion (zero rotation)
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
rot = q.to_axis_angle();
ut_test(fabs(rot(0)) < eps);
ut_test(fabs(rot(1)) < eps);
ut_test(fabs(rot(2)) < eps);
// from axis angle (zero rotation)
rot(0) = rot(1) = rot(2) = 0.0f;
q.from_axis_angle(rot, 0.0f);
q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
ut_test(fabs(q(0) - q_true(0)) < eps);
ut_test(fabs(q(1) - q_true(1)) < eps);
ut_test(fabs(q(2) - q_true(2)) < eps);
ut_test(fabs(q(3) - q_true(3)) < eps);
return true;
}
bool MatrixTest::filterTests(void)
{
const size_t n_x = 6;
const size_t n_y = 5;
SquareMatrix<float, n_x> P = eye<float, n_x>();
SquareMatrix<float, n_y> R = eye<float, n_y>();
Matrix<float, n_y, n_x> C;
C.setIdentity();
float data[] = {1, 2, 3, 4, 5};
Vector<float, n_y> r(data);
Vector<float, n_x> dx;
SquareMatrix<float, n_x> dP;
float beta = 0;
kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0};
Vector<float, n_x> dx_check(data_check);
ut_test(isEqual(dx, dx_check));
return true;
}
bool MatrixTest::helperTests(void)
{
ut_test(fabs(wrap_pi(4.0) - (4.0 - 2 * M_PI)) < 1e-5);
ut_test(fabs(wrap_pi(-4.0) - (-4.0 + 2 * M_PI)) < 1e-5);
ut_test(fabs(wrap_pi(3.0) - (3.0)) < 1e-3);
wrap_pi(NAN);
Vector3f a(1, 2, 3);
Vector3f b(4, 5, 6);
ut_test(isEqual(a, a));
return true;
}
Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u);
Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u)
{
float v = -sinf(t);
return v * ones<float, 6, 1>();
}
bool MatrixTest::integrationTests(void)
{
Vector<float, 6> y = ones<float, 6, 1>();
Vector<float, 3> u = ones<float, 3, 1>();
float t0 = 0;
float tf = 2;
float h = 0.001f;
integrate_rk4(f, y, u, t0, tf, h, y);
float v = 1 + cosf(tf) - cosf(t0);
ut_test(isEqual(y, (ones<float, 6, 1>()*v)));
return true;
}
template class matrix::SquareMatrix<float, 3>;
bool MatrixTest::inverseTests(void)
{
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
float data_check[9] = {
-0.4f, -0.8f, 0.6f,
-0.4f, 4.2f, -2.4f,
0.6f, -2.8f, 1.6f
};
SquareMatrix<float, 3> A(data);
SquareMatrix<float, 3> A_I = inv(A);
SquareMatrix<float, 3> A_I_check(data_check);
float eps = 1e-5;
ut_test((A_I - A_I_check).abs().max() < eps);
SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
inv(zero_test);
return true;
}
bool MatrixTest::matrixAssignmentTests(void)
{
Matrix3f m;
m.setZero();
m(0, 0) = 1;
m(0, 1) = 2;
m(0, 2) = 3;
m(1, 0) = 4;
m(1, 1) = 5;
m(1, 2) = 6;
m(2, 0) = 7;
m(2, 1) = 8;
m(2, 2) = 9;
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data);
double eps = 1e-6f;
for (int i = 0; i < 9; i++) {
ut_test(fabs(data[i] - m2.data()[i]) < eps);
}
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f m3(data_times_2);
ut_test(isEqual(m, m2));
ut_test(!(m == m3));
m2 *= 2;
ut_test(m2 == m3);
m2 /= 2;
m2 -= 1;
float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
ut_test(Matrix3f(data_minus_1) == m2);
m2 += 1;
ut_test(Matrix3f(data) == m2);
m3 -= m2;
ut_test(m3 == m2);
float data_row_02_swap[9] = {
7, 8, 9,
4, 5, 6,
1, 2, 3,
};
float data_col_02_swap[9] = {
3, 2, 1,
6, 5, 4,
9, 8, 7
};
Matrix3f m4(data);
ut_test(-m4 == m4 * (-1));
m4.swapCols(0, 2);
ut_test(m4 == Matrix3f(data_col_02_swap));
m4.swapCols(0, 2);
m4.swapRows(0, 2);
ut_test(m4 == Matrix3f(data_row_02_swap));
ut_test(fabs(m4.min() - 1) < 1e-5);
Scalar<float> s;
s = 1;
ut_test(fabs(s - 1) < 1e-5);
Matrix<float, 1, 1> m5 = s;
ut_test(fabs(m5(0, 0) - s) < 1e-5);
Matrix<float, 2, 2> m6;
m6.setRow(0, Vector2f(1, 1));
m6.setCol(0, Vector2f(1, 1));
return true;
}
bool MatrixTest::matrixMultTests(void)
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I(data_check);
Matrix3f I;
I.setIdentity();
Matrix3f R = A * A_I;
ut_test(isEqual(R, I));
Matrix3f R2 = A;
R2 *= A_I;
ut_test(isEqual(R2, I));
Matrix3f A2 = eye<float, 3>() * 2;
Matrix3f B = A2.emult(A2);
Matrix3f B_check = eye<float, 3>() * 4;
ut_test(isEqual(B, B_check));
return true;
}
bool MatrixTest::matrixScalarMultTests(void)
{
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f A(data);
A = A * 2;
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f A_check(data_check);
ut_test(isEqual(A, A_check));
return true;
}
template class matrix::Matrix<float, 3, 3>;
bool MatrixTest::setIdentityTests(void)
{
Matrix3f A;
A.setIdentity();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
ut_test(fabs(A(i, j) - 1) < 1e-7);
} else {
ut_test(fabs(A(i, j) - 0) < 1e-7);
}
}
}
return true;
}
bool MatrixTest::sliceTests(void)
{
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
float data_check[6] = {
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
Matrix<float, 2, 3> B_check(data_check);
Matrix<float, 2, 3> B(A.slice<2, 3>(1, 0));
ut_test(isEqual(B, B_check));
float data_2[4] = {
11, 12,
13, 14
};
Matrix<float, 2, 2> C(data_2);
A.set(C, 1, 1);
float data_2_check[9] = {
0, 2, 3,
4, 11, 12,
7, 13, 14
};
Matrix<float, 3, 3> D(data_2_check);
ut_test(isEqual(A, D));
return true;
}
bool MatrixTest::squareMatrixTests(void)
{
float data[9] = {1, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
Vector3<float> diag_check(1, 5, 10);
ut_test(isEqual(A.diag(), diag_check));
float data_check[9] = {
1.01158503f, 0.02190432f, 0.03238144f,
0.04349195f, 1.05428524f, 0.06539627f,
0.07576783f, 0.08708946f, 1.10894048f
};
float dt = 0.01f;
SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A * dt), 5);
SquareMatrix<float, 3> eA_check(data_check);
float eps = 1e-3;
ut_test((eA - eA_check).abs().max() < eps);
return true;
}
bool MatrixTest::transposeTests(void)
{
float data[6] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[6] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
ut_test(isEqual(A_T, A_T_check));
return true;
}
bool MatrixTest::vectorTests(void)
{
float data1[] = {1, 2, 3, 4, 5};
float data2[] = {6, 7, 8, 9, 10};
Vector<float, 5> v1(data1);
ut_test(fabs(v1.norm() - 7.416198487095663f) < 1e-5);
Vector<float, 5> v2(data2);
ut_test(fabs(v1.dot(v2) - 130.0f) < 1e-5);
v2.normalize();
Vector<float, 5> v3(v2);
ut_test(v2 == v3);
float data1_sq[] = {1, 4, 9, 16, 25};
Vector<float, 5> v4(data1_sq);
ut_test(v1 == v4.pow(0.5));
return true;
}
bool MatrixTest::vector2Tests(void)
{
Vector2f a(1, 0);
Vector2f b(0, 1);
ut_test(fabs(a % b - 1.0f) < 1e-5);
Vector2f c;
ut_test(fabs(c(0) - 0) < 1e-5);
ut_test(fabs(c(1) - 0) < 1e-5);
Matrix<float, 2, 1> d(a);
ut_test(fabs(d(0, 0) - 1) < 1e-5);
ut_test(fabs(d(1, 0) - 0) < 1e-5);
Vector2f e(d);
ut_test(fabs(e(0) - 1) < 1e-5);
ut_test(fabs(e(1) - 0) < 1e-5);
float data[] = {4, 5};
Vector2f f(data);
ut_test(fabs(f(0) - 4) < 1e-5);
ut_test(fabs(f(1) - 5) < 1e-5);
return true;
}
bool MatrixTest::vector3Tests(void)
{
Vector3f a(1, 0, 0);
Vector3f b(0, 1, 0);
Vector3f c = a.cross(b);
ut_test(c == Vector3f(0, 0, 1));
c = a % b;
ut_test(c == Vector3f(0, 0, 1));
Matrix<float, 3, 1> d(c);
Vector3f e(d);
ut_test(e == d);
float data[] = {4, 5, 6};
Vector3f f(data);
ut_test(f == Vector3f(4, 5, 6));
return true;
}
bool MatrixTest::vectorAssignmentTests(void)
{
Vector3f v;
v(0) = 1;
v(1) = 2;
v(2) = 3;
static const float eps = 1e-7f;
ut_test(fabsf(v(0) - 1) < eps);
ut_test(fabsf(v(1) - 2) < eps);
ut_test(fabsf(v(2) - 3) < eps);
Vector3f v2(4, 5, 6);
ut_test(fabsf(v2(0) - 4) < eps);
ut_test(fabsf(v2(1) - 5) < eps);
ut_test(fabsf(v2(2) - 6) < eps);
SquareMatrix<float, 3> m = diag(Vector3f(1, 2, 3));
ut_test(fabsf(m(0, 0) - 1) < eps);
ut_test(fabsf(m(1, 1) - 2) < eps);
ut_test(fabsf(m(2, 2) - 3) < eps);
return true;
}
src/systemcmds/tests/test_mixer.cpp
+41 -46
View File
@@ -85,22 +85,22 @@ int test_mixer(int argc, char *argv[])
uint16_t servo_predicted[output_max];
int16_t reverse_pwm_mask = 0;
PX4_INFO("testing mixer");
//PX4_INFO("testing mixer");
#if !defined(CONFIG_ARCH_BOARD_SITL)
const char *filename = "/etc/mixers/IO_pass.mix";
#else
const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
#endif
if (argc > 1) {
filename = argv[1];
}
PX4_INFO("loading: %s", filename);
//PX4_INFO("loading: %s", filename);
char buf[2048];
load_mixer_file(filename, &buf[0], sizeof(buf));
unsigned loaded = strlen(buf);
fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
/* load the mixer in chunks, like
* in the case of a remote load,
@@ -114,11 +114,7 @@ int test_mixer(int argc, char *argv[])
/* load at once test */
unsigned xx = loaded;
mixer_group.load_from_buf(&buf[0], xx);
PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8) {
return 1;
}
//ASSERT_EQ(mixer_group.count(), 8);
unsigned empty_load = 2;
char empty_buf[2];
@@ -126,7 +122,9 @@ int test_mixer(int argc, char *argv[])
empty_buf[1] = '\0';
mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load);
PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
//PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
//ASSERT_NE(empty_load, 0);
if (empty_load != 0) {
return 1;
@@ -140,7 +138,7 @@ int test_mixer(int argc, char *argv[])
unsigned transmitted = 0;
PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
//PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
while (transmitted < loaded) {
@@ -155,7 +153,7 @@ int test_mixer(int argc, char *argv[])
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
//fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
@@ -163,7 +161,7 @@ int test_mixer(int argc, char *argv[])
/* if anything was parsed */
if (resid != mixer_text_length) {
fprintf(stderr, "used %u", mixer_text_length - resid);
//fprintf(stderr, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
@@ -176,7 +174,7 @@ int test_mixer(int argc, char *argv[])
transmitted += text_length;
}
PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
//PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8) {
return 1;
@@ -198,7 +196,8 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
/* mix */
should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
@@ -209,7 +208,7 @@ int test_mixer(int argc, char *argv[])
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (i != actuator_controls_s::INDEX_THROTTLE) {
if (r_page_servos[i] < r_page_servo_control_min[i]) {
@@ -233,7 +232,7 @@ int test_mixer(int argc, char *argv[])
actuator_controls[i] = 0.1f;
}
PX4_INFO("ARMING TEST: STARTING RAMP");
//PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
hrt_abstime starttime = hrt_absolute_time();
@@ -250,7 +249,7 @@ int test_mixer(int argc, char *argv[])
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@@ -274,7 +273,7 @@ int test_mixer(int argc, char *argv[])
}
}
PX4_INFO("ARMING TEST: NORMAL OPERATION");
//PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
@@ -292,7 +291,7 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit);
fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@@ -306,7 +305,7 @@ int test_mixer(int argc, char *argv[])
}
}
PX4_INFO("ARMING TEST: DISARMING");
//PX4_INFO("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
@@ -324,7 +323,7 @@ int test_mixer(int argc, char *argv[])
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -337,14 +336,14 @@ int test_mixer(int argc, char *argv[])
sleepcount++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
//printf(".");
//fflush(stdout);
}
}
printf("\n");
//printf("\n");
PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
@@ -366,7 +365,7 @@ int test_mixer(int argc, char *argv[])
/* check ramp */
fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
@@ -388,46 +387,42 @@ int test_mixer(int argc, char *argv[])
sleepcount++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
// printf(".");
// fflush(stdout);
}
}
printf("\n");
//printf("\n");
/* load multirotor at once test */
mixer_group.reset();
if (argc > 2) {
filename = argv[2];
} else {
filename = "/etc/mixers/quad_test.mix";
}
#if !defined(CONFIG_ARCH_BOARD_SITL)
filename = "/etc/mixers/quad_test.mix";
#else
filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
#endif
load_mixer_file(filename, &buf[0], sizeof(buf));
loaded = strlen(buf);
fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
unsigned mc_loaded = loaded;
mixer_group.load_from_buf(&buf[0], mc_loaded);
PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
//PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 5) {
PX4_ERR("FAIL: Quad test mixer load failed");
return 1;
}
PX4_INFO("SUCCESS: No errors in mixer test");
//PX4_INFO("SUCCESS: No errors in mixer test");
return 0;
}
static int
mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control)
mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
{
if (control_group != 0) {
return -1;
src/systemcmds/tests/test_perf.c
+69
View File
@@ -0,0 +1,69 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 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.
*
****************************************************************************/
#include <px4_config.h>
#include <px4_posix.h>
#include <systemlib/perf_counter.h>
#include "tests.h"
int
test_perf(int argc, char *argv[])
{
perf_counter_t cc = perf_alloc(PC_COUNT, "test_count");
perf_counter_t ec = perf_alloc(PC_ELAPSED, "test_elapsed");
if ((cc == NULL) || (ec == NULL)) {
printf("perf: counter alloc failed\n");
return 1;
}
perf_begin(ec);
perf_count(cc);
perf_count(cc);
perf_count(cc);
perf_count(cc);
printf("perf: expect count of 4\n");
perf_print_counter(cc);
perf_end(ec);
printf("perf: expect count of 1\n");
perf_print_counter(ec);
printf("perf: expect at least two counters\n");
perf_print_all(0);
perf_free(cc);
perf_free(ec);
return OK;
}
src/systemcmds/tests/test_uart_baudchange.c
-34
View File
@@ -57,40 +57,6 @@
#include <math.h>
#include <float.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_led
****************************************************************************/
int test_uart_baudchange(int argc, char *argv[])
{
int uart2_nwrite = 0;
src/systemcmds/tests/test_uart_console.c
-34
View File
@@ -56,40 +56,6 @@
#include <float.h>
#include <drivers/drv_hrt.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_led
****************************************************************************/
static void *receive_loop(void *arg)
{
int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);
src/systemcmds/tests/tests.h
+35 -37
View File
@@ -70,50 +70,48 @@
# endif
#endif
/****************************************************************************
* Public Types
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
__BEGIN_DECLS
extern int test_sensors(int argc, char *argv[]);
extern int test_gpio(int argc, char *argv[]);
extern int test_hrt(int argc, char *argv[]);
extern int test_tone(int argc, char *argv[]);
extern int test_led(int argc, char *argv[]);
extern int test_adc(int argc, char *argv[]);
extern int test_int(int argc, char *argv[]);
extern int test_float(int argc, char *argv[]);
extern int test_ppm(int argc, char *argv[]);
extern int test_servo(int argc, char *argv[]);
extern int test_ppm_loopback(int argc, char *argv[]);
extern int test_uart_loopback(int argc, char *argv[]);
extern int test_uart_baudchange(int argc, char *argv[]);
extern int test_cpuload(int argc, char *argv[]);
extern int test_uart_send(int argc, char *argv[]);
extern int test_sleep(int argc, char *argv[]);
extern int test_time(int argc, char *argv[]);
extern int test_uart_console(int argc, char *argv[]);
extern int test_hott_telemetry(int argc, char *argv[]);
extern int test_jig_voltages(int argc, char *argv[]);
extern int test_param(int argc, char *argv[]);
extern int test_autodeclination(int argc, char *argv[]);
extern int test_bson(int argc, char *argv[]);
extern int test_conv(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[]);
extern int test_mount(int argc, char *argv[]);
extern int test_float(int argc, char *argv[]);
extern int test_gpio(int argc, char *argv[]);
extern int test_hott_telemetry(int argc, char *argv[]);
extern int test_hrt(int argc, char *argv[]);
extern int test_int(int argc, char *argv[]);
extern int test_jig_voltages(int argc, char *argv[]);
extern int test_led(int argc, char *argv[]);
extern int test_mathlib(int argc, char *argv[]);
extern int test_eigen(int argc, char *argv[]);
extern int test_matrix(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]);
extern int test_mount(int argc, char *argv[]);
extern int test_param(int argc, char *argv[]);
extern int test_perf(int argc, char *argv[]);
extern int test_ppm(int argc, char *argv[]);
extern int test_ppm_loopback(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]);
extern int test_sensors(int argc, char *argv[]);
extern int test_servo(int argc, char *argv[]);
extern int test_sleep(int argc, char *argv[]);
extern int test_time(int argc, char *argv[]);
extern int test_tone(int argc, char *argv[]);
extern int test_uart_baudchange(int argc, char *argv[]);
extern int test_uart_console(int argc, char *argv[]);
extern int test_uart_loopback(int argc, char *argv[]);
extern int test_uart_send(int argc, char *argv[]);
/* external */
extern int commander_tests_main(int argc, char *argv[]);
extern int mavlink_tests_main(int argc, char *argv[]);
extern int controllib_test_main(int argc, char *argv[]);
extern int uorb_tests_main(int argc, char *argv[]);
extern int rc_tests_main(int argc, char *argv[]);
extern int sf0x_tests_main(int argc, char *argv[]);
__END_DECLS
src/systemcmds/tests/tests_main.c
+68 -176
View File
@@ -38,20 +38,17 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include "tests.h"
#include <px4_config.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <arch/board/board.h>
#include <systemlib/perf_counter.h>
// Not using Eigen at the moment
#define TESTS_EIGEN_DISABLE
@@ -62,8 +59,9 @@
****************************************************************************/
static int test_help(int argc, char *argv[]);
static int test_runner(unsigned option);
static int test_all(int argc, char *argv[]);
static int test_perf(int argc, char *argv[]);
static int test_jig(int argc, char *argv[]);
/****************************************************************************
@@ -78,45 +76,52 @@ const struct {
#define OPT_NOALLTEST (1<<1)
#define OPT_NOJIGTEST (1<<2)
} tests[] = {
#ifdef __PX4_NUTTX
{"led", test_led, 0},
{"time", test_time, OPT_NOJIGTEST},
{"sensors", test_sensors, 0},
{"adc", test_adc, OPT_NOJIGTEST},
#endif /* __PX4_NUTTX */
{"int", test_int, 0},
{"float", test_float, 0},
{"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST},
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST},
{"tone", test_tone, 0},
{"sleep", test_sleep, OPT_NOJIGTEST},
{"perf", test_perf, OPT_NOJIGTEST},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST},
{"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST},
{"param", test_param, 0},
#ifdef __PX4_NUTTX
{"adc", test_adc, OPT_NOJIGTEST},
{"led", test_led, 0},
{"sensors", test_sensors, 0},
{"time", test_time, OPT_NOJIGTEST},
{"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST},
#else
{"rc", rc_tests_main, 0},
#endif /* __PX4_NUTTX */
/* external tests */
{"commander", commander_tests_main, 0},
{"controllib", controllib_test_main, 0},
//{"mavlink", mavlink_tests_main, 0}, // TODO: fix mavlink_tests
{"sf0x", sf0x_tests_main, 0},
{"uorb", uorb_tests_main, 0},
{"autodeclination", test_autodeclination, 0},
{"bson", test_bson, 0},
{"conv", test_conv, 0},
{"file", test_file, OPT_NOJIGTEST | OPT_NOALLTEST},
{"file2", test_file2, OPT_NOJIGTEST},
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
#ifndef TESTS_MATHLIB_DISABLE
{"float", test_float, 0},
{"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
{"int", test_int, 0},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
{"mathlib", test_mathlib, 0},
#endif
#ifndef TESTS_EIGEN_DISABLE
{"eigen", test_eigen, OPT_NOJIGTEST},
#endif
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{"matrix", test_matrix, 0},
{"mixer", test_mixer, OPT_NOJIGTEST},
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
{"param", test_param, 0},
{"perf", test_perf, OPT_NOJIGTEST},
{"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST},
{"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST},
{"sleep", test_sleep, OPT_NOJIGTEST},
{"tone", test_tone, 0},
{"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST},
{NULL, NULL, 0}
};
@@ -138,18 +143,30 @@ test_help(int argc, char *argv[])
static int
test_all(int argc, char *argv[])
{
return test_runner(OPT_NOALLTEST);
}
static int
test_jig(int argc, char *argv[])
{
return test_runner(OPT_NOJIGTEST);
}
static int
test_runner(unsigned option)
{
unsigned i;
char *args[2] = {"all", NULL};
unsigned int failcount = 0;
unsigned int testcount = 0;
bool passed[NTESTS];
unsigned passed[NTESTS];
printf("\nRunning all tests...\n\n");
for (i = 0; tests[i].name; i++) {
/* Only run tests that are not excluded */
if (!(tests[i].options & OPT_NOALLTEST)) {
if (!(tests[i].options & option)) {
for (int j = 0; j < 80; j++) {
printf("-");
}
@@ -164,12 +181,12 @@ test_all(int argc, char *argv[])
fprintf(stderr, " [%s] \t\tFAIL\n", tests[i].name);
fflush(stderr);
failcount++;
passed[i] = false;
passed[i] = 0;
} else {
printf(" [%s] \t\tPASS\n", tests[i].name);
fflush(stdout);
passed[i] = true;
passed[i] = 1;
}
for (int j = 0; j < 80; j++) {
@@ -185,7 +202,7 @@ test_all(int argc, char *argv[])
/* Print summary */
printf("\n");
for (int j = 0; j < 80; j++) {
for (unsigned j = 0; j < 80; j++) {
printf("#");
}
@@ -215,12 +232,12 @@ test_all(int argc, char *argv[])
printf("\n");
/* Print failed tests */
if (failcount > 0) { printf(" Failed tests:\n\n"); }
if (failcount > 0) {
printf(" Failed tests:\n\n");
}
unsigned int k;
for (k = 0; k < i; k++) {
if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) {
for (unsigned k = 0; k < i; k++) {
if (!passed[k] && !(tests[k].options & option)) {
printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
}
}
@@ -230,129 +247,6 @@ test_all(int argc, char *argv[])
return (failcount > 0);
}
static int
test_perf(int argc, char *argv[])
{
perf_counter_t cc, ec;
cc = perf_alloc(PC_COUNT, "test_count");
ec = perf_alloc(PC_ELAPSED, "test_elapsed");
if ((cc == NULL) || (ec == NULL)) {
printf("perf: counter alloc failed\n");
return 1;
}
perf_begin(ec);
perf_count(cc);
perf_count(cc);
perf_count(cc);
perf_count(cc);
printf("perf: expect count of 4\n");
perf_print_counter(cc);
perf_end(ec);
printf("perf: expect count of 1\n");
perf_print_counter(ec);
printf("perf: expect at least two counters\n");
perf_print_all(0);
perf_free(cc);
perf_free(ec);
return OK;
}
int test_jig(int argc, char *argv[])
{
unsigned i;
char *args[2] = {"jig", NULL};
unsigned int failcount = 0;
unsigned int testcount = 0;
bool passed[NTESTS];
printf("\nRunning all tests...\n\n");
for (i = 0; tests[i].name; i++) {
/* Only run tests that are not excluded */
if (!(tests[i].options & OPT_NOJIGTEST)) {
for (int j = 0; j < 80; j++) {
printf("-");
}
printf("\n");
printf(" [%s] \t\tSTARTING TEST\n", tests[i].name);
fflush(stdout);
/* Execute test */
if (tests[i].fn(1, args) != 0) {
fprintf(stderr, " [%s] \t\tFAIL\n", tests[i].name);
fflush(stderr);
failcount++;
passed[i] = false;
} else {
printf(" [%s] \t\tPASS\n", tests[i].name);
fflush(stdout);
passed[i] = true;
}
for (int j = 0; j < 80; j++) {
printf("-");
}
printf("\n");
testcount++;
}
}
/* Print summary */
printf("\n");
for (int j = 0; j < 80; j++) {
printf("-");
}
printf("\n\n");
printf(" T E S T S U M M A R Y\n\n");
if (failcount == 0) {
printf(" ______ __ __ ______ __ __ \n");
printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n");
printf(" \\ \\ __ \\ \\ \\ \\____ \\ \\ \\____ \\ \\ \\/\\ \\ \\ \\ _\"-. \n");
printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n");
printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n");
printf("\n");
printf(" All tests passed (%d of %d)\n", testcount, testcount);
} else {
printf(" ______ ______ __ __ \n");
printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n");
printf(" \\ \\ __\\ \\ \\ __ \\ \\ \\ \\ \\ \\ \\__\n");
printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n");
printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n");
printf("\n");
printf(" Some tests failed (%d of %d)\n", failcount, testcount);
}
printf("\n");
/* Print failed tests */
if (failcount > 0) { printf(" Failed tests:\n\n"); }
for (int k = 0; k < i; k++) {
if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) {
printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
}
}
fflush(stdout);
return 0;
}
__EXPORT int tests_main(int argc, char *argv[]);
/**
@@ -360,19 +254,17 @@ __EXPORT int tests_main(int argc, char *argv[]);
*/
int tests_main(int argc, char *argv[])
{
unsigned i;
if (argc < 2) {
printf("tests: missing test name - 'tests help' for a list of tests\n");
return 1;
}
for (i = 0; tests[i].name; i++) {
for (unsigned i = 0; tests[i].name; i++) {
if (!strcmp(tests[i].name, argv[1])) {
return tests[i].fn(argc - 1, argv + 1);
}
}
printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
return ERROR;
return 1;
}