From a163613cf8dd51dd3ed67a5da2cf354abcc1ccd3 Mon Sep 17 00:00:00 2001 From: Phillip Jones Date: Thu, 13 Aug 2015 22:13:02 -0600 Subject: [PATCH 1/2] This addresses issue #8 by adding unit tests for sensorsAlign(). 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. --- src/main/sensors/boardalignment.c | 4 +- src/test/Makefile | 1 + src/test/unit/alignsensor_unittest.cc | 269 ++++++++++++++++++++++++++ 3 files changed, 272 insertions(+), 2 deletions(-) create mode 100644 src/test/unit/alignsensor_unittest.cc diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index a455936e6..f927ba72f 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -44,9 +44,9 @@ void initBoardAlignment(boardAlignment_t *boardAlignment) standardBoardAlignment = false; fp_angles_t rotationAngles; - rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees); + rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees ); rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees); - rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees); + rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees ); buildRotationMatrix(&rotationAngles, boardRotation); } diff --git a/src/test/Makefile b/src/test/Makefile index 582602d84..c4a6f5fca 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -111,6 +111,7 @@ LIBCLEANFLIGHT_SRC = \ io/serial.c \ rx/rx.c \ sensors/battery.c \ + sensors/boardalignment.c \ telemetry/hott.c LIBCLEANFLIGHT_OBJ = $(LIBCLEANFLIGHT_SRC:%.c=$(OBJECT_DIR)/%.o) diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc new file mode 100644 index 000000000..674ed9572 --- /dev/null +++ b/src/test/unit/alignsensor_unittest.cc @@ -0,0 +1,269 @@ +/* + * 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 . + */ + +#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); +} + From f239eb3f35148fa2451760f411163546da0787fe Mon Sep 17 00:00:00 2001 From: Phillip Jones Date: Thu, 13 Aug 2015 22:13:02 -0600 Subject: [PATCH 2/2] This addresses issue #8 by adding unit tests for sensorsAlign(). 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. --- src/main/sensors/boardalignment.c | 4 +- src/test/Makefile | 1 + src/test/unit/alignsensor_unittest.cc | 269 ++++++++++++++++++++++++++ 3 files changed, 272 insertions(+), 2 deletions(-) create mode 100644 src/test/unit/alignsensor_unittest.cc diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index a455936e6..f927ba72f 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -44,9 +44,9 @@ void initBoardAlignment(boardAlignment_t *boardAlignment) standardBoardAlignment = false; fp_angles_t rotationAngles; - rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees); + rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees ); rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees); - rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees); + rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees ); buildRotationMatrix(&rotationAngles, boardRotation); } diff --git a/src/test/Makefile b/src/test/Makefile index 582602d84..c4a6f5fca 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -111,6 +111,7 @@ LIBCLEANFLIGHT_SRC = \ io/serial.c \ rx/rx.c \ sensors/battery.c \ + sensors/boardalignment.c \ telemetry/hott.c LIBCLEANFLIGHT_OBJ = $(LIBCLEANFLIGHT_SRC:%.c=$(OBJECT_DIR)/%.o) diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc new file mode 100644 index 000000000..674ed9572 --- /dev/null +++ b/src/test/unit/alignsensor_unittest.cc @@ -0,0 +1,269 @@ +/* + * 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 . + */ + +#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); +} +