src/test/unit/alignsensor_unittest.cc

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "math.h"
  19 #include "stdint.h"
  20 #include "time.h"
  21
  22 extern "C" {
  23 #include "common/axis.h"
  24 #include "sensors/boardalignment.h"
  25 #include "sensors/sensors.h"
  26 }
  27
  28 #include "gtest/gtest.h"
  29
  30 /*
  31  * This test file contains an independent method of rotating a vector.
  32  * The output of alignSensor() is compared to the output of the test
  33  * rotation method.
  34  *
  35  * For each alignment condition (CW0, CW90, etc) the source vector under
  36  * test is set to a unit vector along each axis (x-axis, y-axis, z-axis)
  37  * plus one additional random vector is tested.
  38  */
  39
  40 #define DEG2RAD 0.01745329251
  41
  42 static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out)
  43 {
  44     int16_t tmp[3];
  45
  46     for(int i=0; i<3; i++) {
  47         tmp[i] = 0;
  48         for(int j=0; j<3; j++) {
  49             tmp[i] += mat[j][i] * vec[j];
  50         }
  51     }
  52
  53     out[0]=tmp[0];
  54     out[1]=tmp[1];
  55     out[2]=tmp[2];
  56
  57 }
  58
  59 //static void initXAxisRotation(int16_t mat[][3], int16_t angle)
  60 //{
  61 //    mat[0][0] =  1;
  62 //    mat[0][1] =  0;
  63 //    mat[0][2] =  0;
  64 //    mat[1][0] =  0;
  65 //    mat[1][1] =  cos(angle*DEG2RAD);
  66 //    mat[1][2] = -sin(angle*DEG2RAD);
  67 //    mat[2][0] =  0;
  68 //    mat[2][1] =  sin(angle*DEG2RAD);
  69 //    mat[2][2] =  cos(angle*DEG2RAD);
  70 //}
  71
  72 static void initYAxisRotation(int16_t mat[][3], int16_t angle)
  73 {
  74     mat[0][0] =  cos(angle*DEG2RAD);
  75     mat[0][1] =  0;
  76     mat[0][2] =  sin(angle*DEG2RAD);
  77     mat[1][0] =  0;
  78     mat[1][1] =  1;
  79     mat[1][2] =  0;
  80     mat[2][0] = -sin(angle*DEG2RAD);
  81     mat[2][1] =  0;
  82     mat[2][2] =  cos(angle*DEG2RAD);
  83 }
  84
  85 static void initZAxisRotation(int16_t mat[][3], int16_t angle)
  86 {
  87     mat[0][0] =  cos(angle*DEG2RAD);
  88     mat[0][1] = -sin(angle*DEG2RAD);
  89     mat[0][2] =  0;
  90     mat[1][0] =  sin(angle*DEG2RAD);
  91     mat[1][1] =  cos(angle*DEG2RAD);
  92     mat[1][2] =  0;
  93     mat[2][0] =  0;
  94     mat[2][1] =  0;
  95     mat[2][2] =  1;
  96 }
  97
  98 static void testCW(sensor_align_e rotation, int16_t angle)
  99 {
 100     int16_t src[XYZ_AXIS_COUNT];
 101     int16_t dest[XYZ_AXIS_COUNT];
 102     int16_t test[XYZ_AXIS_COUNT];
 103
 104     // unit vector along x-axis
 105     src[X] = 1;
 106     src[Y] = 0;
 107     src[Z] = 0;
 108
 109     int16_t matrix[3][3];
 110     initZAxisRotation(matrix, angle);
 111     rotateVector(matrix, src, test);
 112
 113     alignSensors(src, dest, rotation);
 114     EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 115     EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 116     EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 117
 118     // unit vector along y-axis
 119     src[X] = 0;
 120     src[Y] = 1;
 121     src[Z] = 0;
 122
 123     rotateVector(matrix, src, test);
 124     alignSensors(src, dest, rotation);
 125     EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 126     EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 127     EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 128
 129     // unit vector along z-axis
 130     src[X] = 0;
 131     src[Y] = 0;
 132     src[Z] = 1;
 133
 134     rotateVector(matrix, src, test);
 135     alignSensors(src, dest, rotation);
 136     EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 137     EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 138     EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 139
 140     // random vector to test
 141     src[X] = rand() % 5;
 142     src[Y] = rand() % 5;
 143     src[Z] = rand() % 5;
 144
 145     rotateVector(matrix, src, test);
 146     alignSensors(src, dest, rotation);
 147     EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 148     EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 149     EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 150 }
 151
 152 /*
 153  * Since the order of flip and rotation matters, these tests make the
 154  * assumption that the 'flip' occurs first, followed by clockwise rotation
 155  */
 156 static void testCWFlip(sensor_align_e rotation, int16_t angle)
 157 {
 158     int16_t src[XYZ_AXIS_COUNT];
 159     int16_t dest[XYZ_AXIS_COUNT];
 160     int16_t test[XYZ_AXIS_COUNT];
 161
 162     // unit vector along x-axis
 163     src[X] = 1;
 164     src[Y] = 0;
 165     src[Z] = 0;
 166
 167     int16_t matrix[3][3];
 168     initYAxisRotation(matrix, 180);
 169     rotateVector(matrix, src, test);
 170     initZAxisRotation(matrix, angle);
 171     rotateVector(matrix, test, test);
 172
 173     alignSensors(src, dest, rotation);
 174
 175     EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 176     EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 177     EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 178
 179     // unit vector along y-axis
 180     src[X] = 0;
 181     src[Y] = 1;
 182     src[Z] = 0;
 183
 184     initYAxisRotation(matrix, 180);
 185     rotateVector(matrix, src, test);
 186     initZAxisRotation(matrix, angle);
 187     rotateVector(matrix, test, test);
 188
 189     alignSensors(src, dest, rotation);
 190
 191     EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 192     EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 193     EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 194
 195     // unit vector along z-axis
 196     src[X] = 0;
 197     src[Y] = 0;
 198     src[Z] = 1;
 199
 200     initYAxisRotation(matrix, 180);
 201     rotateVector(matrix, src, test);
 202     initZAxisRotation(matrix, angle);
 203     rotateVector(matrix, test, test);
 204
 205     alignSensors(src, dest, rotation);
 206
 207     EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 208     EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 209     EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 210
 211      // random vector to test
 212     src[X] = rand() % 5;
 213     src[Y] = rand() % 5;
 214     src[Z] = rand() % 5;
 215
 216     initYAxisRotation(matrix, 180);
 217     rotateVector(matrix, src, test);
 218     initZAxisRotation(matrix, angle);
 219     rotateVector(matrix, test, test);
 220
 221     alignSensors(src, dest, rotation);
 222
 223     EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
 224     EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
 225     EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
 226 }
 227
 228
 229 TEST(AlignSensorTest, ClockwiseZeroDegrees)
 230 {
 231     srand(time(NULL));
 232     testCW(CW0_DEG, 0);
 233 }
 234
 235 TEST(AlignSensorTest, ClockwiseNinetyDegrees)
 236 {
 237     testCW(CW90_DEG, 90);
 238 }
 239
 240 TEST(AlignSensorTest, ClockwiseOneEightyDegrees)
 241 {
 242     testCW(CW180_DEG, 180);
 243 }
 244
 245 TEST(AlignSensorTest, ClockwiseTwoSeventyDegrees)
 246 {
 247     testCW(CW270_DEG, 270);
 248 }
 249
 250 TEST(AlignSensorTest, ClockwiseZeroDegreesFlip)
 251 {
 252     testCWFlip(CW0_DEG_FLIP, 0);
 253 }
 254
 255 TEST(AlignSensorTest, ClockwiseNinetyDegreesFlip)
 256 {
 257     testCWFlip(CW90_DEG_FLIP, 90);
 258 }
 259
 260 TEST(AlignSensorTest, ClockwiseOneEightyDegreesFlip)
 261 {
 262     testCWFlip(CW180_DEG_FLIP, 180);
 263 }
 264
 265 TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
 266 {
 267     testCWFlip(CW270_DEG_FLIP, 270);
 268 }
 269