mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 13:25:30 +03:00
f239eb3f35
The test file includes a simple independent implementation of
vector rotation. Each of the following sensor_align_e types are
tested:
* CW0_DEG
* CW90_DEG
* CW180_DEG
* CW270_DEG
* CW0_DEG_FLIP
* CW90_DEG_FLIP
* CW180_DEG_FLIP
* CW270_DEG_FLIP
For each test, three unit vectors and a random vector are tested.
* {1, 0, 0}
* {0, 1, 0}
* {0, 0, 1}
* {R, R, R} (where R is a random number)
The vector under test is rotated using the functions defined in the
test file. The output of the test function is compared to the
output of the sensorsAlign() function. The outputs match for all
test conditions.
269 lines
8.1 KiB
C++
269 lines
8.1 KiB
C++
/*
|
|
* This file is part of Cleanflight.
|
|
*
|
|
* Cleanflight is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Cleanflight is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "math.h"
|
|
#include "stdint.h"
|
|
#include "time.h"
|
|
|
|
extern "C" {
|
|
#include "common/axis.h"
|
|
#include "sensors/boardalignment.h"
|
|
#include "sensors/sensors.h"
|
|
}
|
|
|
|
#include "gtest/gtest.h"
|
|
|
|
/*
|
|
* This test file contains an independent method of rotating a vector.
|
|
* The output of alignSensor() is compared to the output of the test
|
|
* rotation method.
|
|
*
|
|
* For each alignment condition (CW0, CW90, etc) the source vector under
|
|
* test is set to a unit vector along each axis (x-axis, y-axis, z-axis)
|
|
* plus one additional random vector is tested.
|
|
*/
|
|
|
|
#define DEG2RAD 0.01745329251
|
|
|
|
static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out)
|
|
{
|
|
int16_t tmp[3];
|
|
|
|
for(int i=0; i<3; i++) {
|
|
tmp[i] = 0;
|
|
for(int j=0; j<3; j++) {
|
|
tmp[i] += mat[j][i] * vec[j];
|
|
}
|
|
}
|
|
|
|
out[0]=tmp[0];
|
|
out[1]=tmp[1];
|
|
out[2]=tmp[2];
|
|
|
|
}
|
|
|
|
//static void initXAxisRotation(int16_t mat[][3], int16_t angle)
|
|
//{
|
|
// mat[0][0] = 1;
|
|
// mat[0][1] = 0;
|
|
// mat[0][2] = 0;
|
|
// mat[1][0] = 0;
|
|
// mat[1][1] = cos(angle*DEG2RAD);
|
|
// mat[1][2] = -sin(angle*DEG2RAD);
|
|
// mat[2][0] = 0;
|
|
// mat[2][1] = sin(angle*DEG2RAD);
|
|
// mat[2][2] = cos(angle*DEG2RAD);
|
|
//}
|
|
|
|
static void initYAxisRotation(int16_t mat[][3], int16_t angle)
|
|
{
|
|
mat[0][0] = cos(angle*DEG2RAD);
|
|
mat[0][1] = 0;
|
|
mat[0][2] = sin(angle*DEG2RAD);
|
|
mat[1][0] = 0;
|
|
mat[1][1] = 1;
|
|
mat[1][2] = 0;
|
|
mat[2][0] = -sin(angle*DEG2RAD);
|
|
mat[2][1] = 0;
|
|
mat[2][2] = cos(angle*DEG2RAD);
|
|
}
|
|
|
|
static void initZAxisRotation(int16_t mat[][3], int16_t angle)
|
|
{
|
|
mat[0][0] = cos(angle*DEG2RAD);
|
|
mat[0][1] = -sin(angle*DEG2RAD);
|
|
mat[0][2] = 0;
|
|
mat[1][0] = sin(angle*DEG2RAD);
|
|
mat[1][1] = cos(angle*DEG2RAD);
|
|
mat[1][2] = 0;
|
|
mat[2][0] = 0;
|
|
mat[2][1] = 0;
|
|
mat[2][2] = 1;
|
|
}
|
|
|
|
static void testCW(sensor_align_e rotation, int16_t angle)
|
|
{
|
|
int16_t src[XYZ_AXIS_COUNT];
|
|
int16_t dest[XYZ_AXIS_COUNT];
|
|
int16_t test[XYZ_AXIS_COUNT];
|
|
|
|
// unit vector along x-axis
|
|
src[X] = 1;
|
|
src[Y] = 0;
|
|
src[Z] = 0;
|
|
|
|
int16_t matrix[3][3];
|
|
initZAxisRotation(matrix, angle);
|
|
rotateVector(matrix, src, test);
|
|
|
|
alignSensors(src, dest, rotation);
|
|
EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
|
|
// unit vector along y-axis
|
|
src[X] = 0;
|
|
src[Y] = 1;
|
|
src[Z] = 0;
|
|
|
|
rotateVector(matrix, src, test);
|
|
alignSensors(src, dest, rotation);
|
|
EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
|
|
// unit vector along z-axis
|
|
src[X] = 0;
|
|
src[Y] = 0;
|
|
src[Z] = 1;
|
|
|
|
rotateVector(matrix, src, test);
|
|
alignSensors(src, dest, rotation);
|
|
EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
|
|
// random vector to test
|
|
src[X] = rand() % 5;
|
|
src[Y] = rand() % 5;
|
|
src[Z] = rand() % 5;
|
|
|
|
rotateVector(matrix, src, test);
|
|
alignSensors(src, dest, rotation);
|
|
EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
}
|
|
|
|
/*
|
|
* Since the order of flip and rotation matters, these tests make the
|
|
* assumption that the 'flip' occurs first, followed by clockwise rotation
|
|
*/
|
|
static void testCWFlip(sensor_align_e rotation, int16_t angle)
|
|
{
|
|
int16_t src[XYZ_AXIS_COUNT];
|
|
int16_t dest[XYZ_AXIS_COUNT];
|
|
int16_t test[XYZ_AXIS_COUNT];
|
|
|
|
// unit vector along x-axis
|
|
src[X] = 1;
|
|
src[Y] = 0;
|
|
src[Z] = 0;
|
|
|
|
int16_t matrix[3][3];
|
|
initYAxisRotation(matrix, 180);
|
|
rotateVector(matrix, src, test);
|
|
initZAxisRotation(matrix, angle);
|
|
rotateVector(matrix, test, test);
|
|
|
|
alignSensors(src, dest, rotation);
|
|
|
|
EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
|
|
// unit vector along y-axis
|
|
src[X] = 0;
|
|
src[Y] = 1;
|
|
src[Z] = 0;
|
|
|
|
initYAxisRotation(matrix, 180);
|
|
rotateVector(matrix, src, test);
|
|
initZAxisRotation(matrix, angle);
|
|
rotateVector(matrix, test, test);
|
|
|
|
alignSensors(src, dest, rotation);
|
|
|
|
EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
|
|
// unit vector along z-axis
|
|
src[X] = 0;
|
|
src[Y] = 0;
|
|
src[Z] = 1;
|
|
|
|
initYAxisRotation(matrix, 180);
|
|
rotateVector(matrix, src, test);
|
|
initZAxisRotation(matrix, angle);
|
|
rotateVector(matrix, test, test);
|
|
|
|
alignSensors(src, dest, rotation);
|
|
|
|
EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
|
|
// random vector to test
|
|
src[X] = rand() % 5;
|
|
src[Y] = rand() % 5;
|
|
src[Z] = rand() % 5;
|
|
|
|
initYAxisRotation(matrix, 180);
|
|
rotateVector(matrix, src, test);
|
|
initZAxisRotation(matrix, angle);
|
|
rotateVector(matrix, test, test);
|
|
|
|
alignSensors(src, dest, rotation);
|
|
|
|
EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
|
|
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
|
|
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
|
|
}
|
|
|
|
|
|
TEST(AlignSensorTest, ClockwiseZeroDegrees)
|
|
{
|
|
srand(time(NULL));
|
|
testCW(CW0_DEG, 0);
|
|
}
|
|
|
|
TEST(AlignSensorTest, ClockwiseNinetyDegrees)
|
|
{
|
|
testCW(CW90_DEG, 90);
|
|
}
|
|
|
|
TEST(AlignSensorTest, ClockwiseOneEightyDegrees)
|
|
{
|
|
testCW(CW180_DEG, 180);
|
|
}
|
|
|
|
TEST(AlignSensorTest, ClockwiseTwoSeventyDegrees)
|
|
{
|
|
testCW(CW270_DEG, 270);
|
|
}
|
|
|
|
TEST(AlignSensorTest, ClockwiseZeroDegreesFlip)
|
|
{
|
|
testCWFlip(CW0_DEG_FLIP, 0);
|
|
}
|
|
|
|
TEST(AlignSensorTest, ClockwiseNinetyDegreesFlip)
|
|
{
|
|
testCWFlip(CW90_DEG_FLIP, 90);
|
|
}
|
|
|
|
TEST(AlignSensorTest, ClockwiseOneEightyDegreesFlip)
|
|
{
|
|
testCWFlip(CW180_DEG_FLIP, 180);
|
|
}
|
|
|
|
TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
|
|
{
|
|
testCWFlip(CW270_DEG_FLIP, 270);
|
|
}
|
|
|