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 "common/sensor_alignment.h"
  25 #include "common/sensor_alignment_impl.h"
  26 #include "drivers/sensor.h"
  27 #include "sensors/boardalignment.h"
  28 #include "sensors/sensors.h"
  29 }
  30
  31 #include "gtest/gtest.h"
  32
  33 /*
  34  * This test file contains an independent method of rotating a vector.
  35  * The output of alignSensor() is compared to the output of the test
  36  * rotation method.
  37  *
  38  * For each alignment condition (ALIGN_CW0, CW90, etc) the source vector under
  39  * test is set to a unit vector along each axis (x-axis, y-axis, z-axis)
  40  * plus one additional random vector is tested.
  41  */
  42
  43 #define DEG2RAD 0.01745329251
  44
  45 static void rotateVector(int32_t mat[3][3], float vec[3], float *out)
  46 {
  47     float tmp[3];
  48
  49     for(int i=0; i<3; i++) {
  50         tmp[i] = 0;
  51         for(int j=0; j<3; j++) {
  52             tmp[i] += mat[j][i] * vec[j];
  53         }
  54     }
  55
  56     out[0]=tmp[0];
  57     out[1]=tmp[1];
  58     out[2]=tmp[2];
  59
  60 }
  61
  62 //static void initXAxisRotation(int32_t mat[][3], int32_t angle)
  63 //{
  64 //    mat[0][0] =  1;
  65 //    mat[0][1] =  0;
  66 //    mat[0][2] =  0;
  67 //    mat[1][0] =  0;
  68 //    mat[1][1] =  cos(angle*DEG2RAD);
  69 //    mat[1][2] = -sin(angle*DEG2RAD);
  70 //    mat[2][0] =  0;
  71 //    mat[2][1] =  sin(angle*DEG2RAD);
  72 //    mat[2][2] =  cos(angle*DEG2RAD);
  73 //}
  74
  75 static void initYAxisRotation(int32_t mat[][3], int32_t angle)
  76 {
  77     mat[0][0] =  cos(angle*DEG2RAD);
  78     mat[0][1] =  0;
  79     mat[0][2] =  sin(angle*DEG2RAD);
  80     mat[1][0] =  0;
  81     mat[1][1] =  1;
  82     mat[1][2] =  0;
  83     mat[2][0] = -sin(angle*DEG2RAD);
  84     mat[2][1] =  0;
  85     mat[2][2] =  cos(angle*DEG2RAD);
  86 }
  87
  88 static void initZAxisRotation(int32_t mat[][3], int32_t angle)
  89 {
  90     mat[0][0] =  cos(angle*DEG2RAD);
  91     mat[0][1] = -sin(angle*DEG2RAD);
  92     mat[0][2] =  0;
  93     mat[1][0] =  sin(angle*DEG2RAD);
  94     mat[1][1] =  cos(angle*DEG2RAD);
  95     mat[1][2] =  0;
  96     mat[2][0] =  0;
  97     mat[2][1] =  0;
  98     mat[2][2] =  1;
  99 }
 100
 101 #define TOL 1e-5 // TOLERANCE
 102
 103 static void alignSensorViaMatrixFromRotation(float *dest, sensor_align_e alignment)
 104 {
 105     fp_rotationMatrix_t sensorRotationMatrix;
 106
 107     sensorAlignment_t sensorAlignment;
 108
 109     buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
 110
 111     buildRotationMatrixFromAlignment(&sensorAlignment, &sensorRotationMatrix);
 112
 113     alignSensorViaMatrix(dest, &sensorRotationMatrix);
 114 }
 115
 116 static void testCW(sensor_align_e rotation, int32_t angle)
 117 {
 118     float src[XYZ_AXIS_COUNT];
 119     float test[XYZ_AXIS_COUNT];
 120
 121     // unit vector along x-axis
 122     src[X] = 1;
 123     src[Y] = 0;
 124     src[Z] = 0;
 125
 126     int32_t matrix[3][3];
 127     initZAxisRotation(matrix, angle);
 128     rotateVector(matrix, src, test);
 129
 130     alignSensorViaMatrixFromRotation(src, rotation);
 131     EXPECT_NEAR(test[X], src[X], TOL) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 132     EXPECT_NEAR(test[Y], src[Y], TOL) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 133     EXPECT_NEAR(test[Z], src[Z], TOL) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 134
 135     // unit vector along y-axis
 136     src[X] = 0;
 137     src[Y] = 1;
 138     src[Z] = 0;
 139
 140     rotateVector(matrix, src, test);
 141     alignSensorViaMatrixFromRotation(src, rotation);
 142     EXPECT_NEAR(test[X], src[X], TOL) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 143     EXPECT_NEAR(test[Y], src[Y], TOL) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 144     EXPECT_NEAR(test[Z], src[Z], TOL) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 145
 146     // unit vector along z-axis
 147     src[X] = 0;
 148     src[Y] = 0;
 149     src[Z] = 1;
 150
 151     rotateVector(matrix, src, test);
 152     alignSensorViaMatrixFromRotation(src, rotation);
 153     EXPECT_NEAR(test[X], src[X], TOL) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 154     EXPECT_NEAR(test[Y], src[Y], TOL) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 155     EXPECT_NEAR(test[Z], src[Z], TOL) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 156
 157     // random vector to test
 158     src[X] = rand() % 5;
 159     src[Y] = rand() % 5;
 160     src[Z] = rand() % 5;
 161
 162     rotateVector(matrix, src, test);
 163     alignSensorViaMatrixFromRotation(src,  rotation);
 164     EXPECT_NEAR(test[X], src[X], TOL) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
 165     EXPECT_NEAR(test[Y], src[Y], TOL) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 166     EXPECT_NEAR(test[Z], src[Z], TOL) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 167 }
 168
 169 /*
 170  * Since the order of flip and rotation matters, these tests make the
 171  * assumption that the 'flip' occurs first, followed by clockwise rotation
 172  */
 173 static void testCWFlip(sensor_align_e rotation, int32_t angle)
 174 {
 175     float src[XYZ_AXIS_COUNT];
 176     float test[XYZ_AXIS_COUNT];
 177
 178     // unit vector along x-axis
 179     src[X] = 1;
 180     src[Y] = 0;
 181     src[Z] = 0;
 182
 183     int32_t matrix[3][3];
 184     initYAxisRotation(matrix, 180);
 185     rotateVector(matrix, src, test);
 186     initZAxisRotation(matrix, angle);
 187     rotateVector(matrix, test, test);
 188
 189     alignSensorViaMatrixFromRotation(src, rotation);
 190
 191     EXPECT_NEAR(test[X], src[X], TOL) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 192     EXPECT_NEAR(test[Y], src[Y], TOL) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 193     EXPECT_NEAR(test[Z], src[Z], TOL) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 194
 195     // unit vector along y-axis
 196     src[X] = 0;
 197     src[Y] = 1;
 198     src[Z] = 0;
 199
 200     initYAxisRotation(matrix, 180);
 201     rotateVector(matrix, src, test);
 202     initZAxisRotation(matrix, angle);
 203     rotateVector(matrix, test, test);
 204
 205     alignSensorViaMatrixFromRotation(src, rotation);
 206
 207     EXPECT_NEAR(test[X], src[X], TOL) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 208     EXPECT_NEAR(test[Y], src[Y], TOL) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 209     EXPECT_NEAR(test[Z], src[Z], TOL) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 210
 211     // unit vector along z-axis
 212     src[X] = 0;
 213     src[Y] = 0;
 214     src[Z] = 1;
 215
 216     initYAxisRotation(matrix, 180);
 217     rotateVector(matrix, src, test);
 218     initZAxisRotation(matrix, angle);
 219     rotateVector(matrix, test, test);
 220
 221     alignSensorViaMatrixFromRotation(src, rotation);
 222
 223     EXPECT_NEAR(test[X], src[X], TOL) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 224     EXPECT_NEAR(test[Y], src[Y], TOL) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 225     EXPECT_NEAR(test[Z], src[Z], TOL) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 226
 227      // random vector to test
 228     src[X] = rand() % 5;
 229     src[Y] = rand() % 5;
 230     src[Z] = rand() % 5;
 231
 232     initYAxisRotation(matrix, 180);
 233     rotateVector(matrix, src, test);
 234     initZAxisRotation(matrix, angle);
 235     rotateVector(matrix, test, test);
 236
 237     alignSensorViaMatrixFromRotation(src, rotation);
 238
 239     EXPECT_NEAR(test[X], src[X], TOL) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
 240     EXPECT_NEAR(test[Y], src[Y], TOL) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 241     EXPECT_NEAR(test[Z], src[Z], TOL) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 242 }
 243
 244
 245 TEST(AlignSensorTest, ClockwiseZeroDegrees)
 246 {
 247     srand(time(NULL));
 248     testCW(CW0_DEG, 0);
 249 }
 250
 251 TEST(AlignSensorTest, ClockwiseNinetyDegrees)
 252 {
 253     testCW(CW90_DEG, 90);
 254 }
 255
 256 TEST(AlignSensorTest, ClockwiseOneEightyDegrees)
 257 {
 258     testCW(CW180_DEG, 180);
 259 }
 260
 261 TEST(AlignSensorTest, ClockwiseTwoSeventyDegrees)
 262 {
 263     testCW(CW270_DEG, 270);
 264 }
 265
 266 TEST(AlignSensorTest, ClockwiseZeroDegreesFlip)
 267 {
 268     testCWFlip(CW0_DEG_FLIP, 0);
 269 }
 270
 271 TEST(AlignSensorTest, ClockwiseNinetyDegreesFlip)
 272 {
 273     testCWFlip(CW90_DEG_FLIP, 90);
 274 }
 275
 276 TEST(AlignSensorTest, ClockwiseOneEightyDegreesFlip)
 277 {
 278     testCWFlip(CW180_DEG_FLIP, 180);
 279 }
 280
 281 TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
 282 {
 283     testCWFlip(CW270_DEG_FLIP, 270);
 284 }
 285
 286 static void testBuildAlignmentWithStandardAlignment(sensor_align_e alignment, sensorAlignment_t expectedSensorAlignment)
 287 {
 288     sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(6, 6, 6);
 289
 290     buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
 291
 292     for (int i = 0; i < (int)(sizeof(sensorAlignment.raw) / sizeof(sensorAlignment.raw[0])); i++) {
 293         EXPECT_EQ(expectedSensorAlignment.raw[i], sensorAlignment.raw[i]) << "Sensor alignment was not updated. alignment: " << alignment;
 294     }
 295 }
 296
 297 TEST(AlignSensorTest, AttemptBuildAlignmentWithStandardAlignment)
 298 {
 299     testBuildAlignmentWithStandardAlignment(CW0_DEG,           CUSTOM_ALIGN_CW0_DEG);
 300     testBuildAlignmentWithStandardAlignment(CW90_DEG,          CUSTOM_ALIGN_CW90_DEG);
 301     testBuildAlignmentWithStandardAlignment(CW180_DEG,         CUSTOM_ALIGN_CW180_DEG);
 302     testBuildAlignmentWithStandardAlignment(CW270_DEG,         CUSTOM_ALIGN_CW270_DEG);
 303     testBuildAlignmentWithStandardAlignment(CW0_DEG_FLIP,      CUSTOM_ALIGN_CW0_DEG_FLIP);
 304     testBuildAlignmentWithStandardAlignment(CW90_DEG_FLIP,     CUSTOM_ALIGN_CW90_DEG_FLIP);
 305     testBuildAlignmentWithStandardAlignment(CW180_DEG_FLIP,    CUSTOM_ALIGN_CW180_DEG_FLIP);
 306     testBuildAlignmentWithStandardAlignment(CW270_DEG_FLIP,    CUSTOM_ALIGN_CW270_DEG_FLIP);
 307 }
 308
 309 TEST(AlignSensorTest, AttemptBuildAlignmentFromCustomAlignment)
 310 {
 311     sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
 312
 313     buildAlignmentFromStandardAlignment(&sensorAlignment, ALIGN_CUSTOM);
 314
 315     sensorAlignment_t expectedSensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
 316
 317     for (int i = 0; i < (int)(sizeof(sensorAlignment.raw) / sizeof(sensorAlignment.raw[0])); i++) {
 318         EXPECT_EQ(expectedSensorAlignment.raw[i], sensorAlignment.raw[i]) << "Custom alignment should not be updated.";
 319     }
 320 }
 321
 322 TEST(AlignSensorTest, AttemptBuildAlignmentFromDefaultAlignment)
 323 {
 324     sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
 325
 326     buildAlignmentFromStandardAlignment(&sensorAlignment, ALIGN_DEFAULT);
 327
 328     sensorAlignment_t expectedSensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
 329
 330     for (int i = 0; i < (int)(sizeof(sensorAlignment.raw) / sizeof(sensorAlignment.raw[0])); i++) {
 331         EXPECT_EQ(expectedSensorAlignment.raw[i], sensorAlignment.raw[i]) << "Default alignment should not be updated.";
 332     }
 333 }
 334
 335 TEST(AlignSensorTest, AlignmentBitmasks)
 336 {
 337     uint8_t bits;
 338
 339     bits = ALIGNMENT_TO_BITMASK(CW0_DEG);
 340     EXPECT_EQ(0x0, bits); // 000000
 341     EXPECT_EQ(0, ALIGNMENT_YAW_ROTATIONS(bits));
 342     EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
 343     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 344
 345     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 346     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 347     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 348
 349     bits = ALIGNMENT_TO_BITMASK(CW90_DEG);
 350     EXPECT_EQ(0x1, bits); // 000001
 351     EXPECT_EQ(1, ALIGNMENT_YAW_ROTATIONS(bits));
 352     EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
 353     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 354
 355     EXPECT_EQ(1, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 356     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 357     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 358
 359     bits = ALIGNMENT_TO_BITMASK(CW180_DEG);
 360     EXPECT_EQ(0x2, bits); // 000010
 361     EXPECT_EQ(2, ALIGNMENT_YAW_ROTATIONS(bits));
 362     EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
 363     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 364
 365     EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 366     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 367     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 368
 369     bits = ALIGNMENT_TO_BITMASK(CW270_DEG);
 370     EXPECT_EQ(0x3, bits); // 000011
 371     EXPECT_EQ(3, ALIGNMENT_YAW_ROTATIONS(bits));
 372     EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
 373     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 374
 375     EXPECT_EQ(3, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 376     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 377     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 378
 379     bits = ALIGNMENT_TO_BITMASK(CW0_DEG_FLIP);
 380     EXPECT_EQ(0x8, bits); // 001000
 381     EXPECT_EQ(0, ALIGNMENT_YAW_ROTATIONS(bits));
 382     EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
 383     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 384
 385     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 386     EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 387     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 388
 389     bits = ALIGNMENT_TO_BITMASK(CW90_DEG_FLIP);
 390     EXPECT_EQ(0x9, bits); // 001001
 391     EXPECT_EQ(1, ALIGNMENT_YAW_ROTATIONS(bits));
 392     EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
 393     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 394
 395     EXPECT_EQ(1, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 396     EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 397     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 398
 399     bits = ALIGNMENT_TO_BITMASK(CW180_DEG_FLIP);
 400     EXPECT_EQ(0xA, bits); // 001010
 401     EXPECT_EQ(2, ALIGNMENT_YAW_ROTATIONS(bits));
 402     EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
 403     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 404
 405     EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 406     EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 407     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 408
 409     bits = ALIGNMENT_TO_BITMASK(CW270_DEG_FLIP);
 410     EXPECT_EQ(0xB, bits); // 001011
 411     EXPECT_EQ(3, ALIGNMENT_YAW_ROTATIONS(bits));
 412     EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
 413     EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
 414
 415     EXPECT_EQ(3, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
 416     EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
 417     EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
 418 }