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