src/test/unit/alignsensor_unittest.cc.txt

   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 "drivers/sensor.h"
  25 #include "sensors/boardalignment.h"
  26 #include "sensors/sensors.h"
  27 }
  28
  29 #include "gtest/gtest.h"
  30
  31 /*
  32  * This test file contains an independent method of rotating a vector.
  33  * The output of applySensorAlignment() is compared to the output of the test
  34  * rotation method.
  35  *
  36  * For each alignment condition (CW0, CW90, etc) the source vector under
  37  * test is set to a unit vector along each axis (x-axis, y-axis, z-axis)
  38  * plus one additional random vector is tested.
  39  */
  40
  41 #define DEG2RAD 0.01745329251
  42
  43 static void rotateVector(float mat[3][3], float vec[3], float *out)
  44 {
  45     float tmp[3];
  46
  47     for(int i=0; i<3; i++) {
  48         tmp[i] = 0;
  49         for(int j=0; j<3; j++) {
  50             tmp[i] += mat[j][i] * vec[j];
  51         }
  52     }
  53
  54     out[0]=tmp[0];
  55     out[1]=tmp[1];
  56     out[2]=tmp[2];
  57
  58 }
  59
  60 //static void initXAxisRotation(int32_t mat[][3], int32_t angle)
  61 //{
  62 //    mat[0][0] =  1;
  63 //    mat[0][1] =  0;
  64 //    mat[0][2] =  0;
  65 //    mat[1][0] =  0;
  66 //    mat[1][1] =  cos(angle*DEG2RAD);
  67 //    mat[1][2] = -sin(angle*DEG2RAD);
  68 //    mat[2][0] =  0;
  69 //    mat[2][1] =  sin(angle*DEG2RAD);
  70 //    mat[2][2] =  cos(angle*DEG2RAD);
  71 //}
  72
  73 static void initYAxisRotation(float mat[][3], int32_t angle)
  74 {
  75     mat[0][0] =  cos(angle*DEG2RAD);
  76     mat[0][1] =  0;
  77     mat[0][2] =  sin(angle*DEG2RAD);
  78     mat[1][0] =  0;
  79     mat[1][1] =  1;
  80     mat[1][2] =  0;
  81     mat[2][0] = -sin(angle*DEG2RAD);
  82     mat[2][1] =  0;
  83     mat[2][2] =  cos(angle*DEG2RAD);
  84 }
  85
  86 static void initZAxisRotation(float mat[][3], int32_t angle)
  87 {
  88     mat[0][0] =  cos(angle*DEG2RAD);
  89     mat[0][1] = -sin(angle*DEG2RAD);
  90     mat[0][2] =  0;
  91     mat[1][0] =  sin(angle*DEG2RAD);
  92     mat[1][1] =  cos(angle*DEG2RAD);
  93     mat[1][2] =  0;
  94     mat[2][0] =  0;
  95     mat[2][1] =  0;
  96     mat[2][2] =  1;
  97 }
  98
  99 static void testCW(sensor_align_e rotation, int32_t angle)
 100 {
 101     float src[XYZ_AXIS_COUNT];
 102     float 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     float matrix[3][3];
 110     initZAxisRotation(matrix, angle);
 111     rotateVector(matrix, src, test);
 112
 113     applySensorAlignment(src, src, rotation);
 114     EXPECT_EQ(test[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 115     EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 116     EXPECT_EQ(test[Z], src[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[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     applySensorAlignment(src, src, rotation);
 125     EXPECT_EQ(test[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 126     EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 127     EXPECT_EQ(test[Z], src[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[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     applySensorAlignment(src, src, rotation);
 136     EXPECT_EQ(test[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 137     EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 138     EXPECT_EQ(test[Z], src[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[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     applySensorAlignment(src, src, rotation);
 147     EXPECT_EQ(test[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
 148     EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 149     EXPECT_EQ(test[Z], src[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[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, int32_t angle)
 157 {
 158     float src[XYZ_AXIS_COUNT];
 159     float test[XYZ_AXIS_COUNT];
 160
 161     // unit vector along x-axis
 162     src[X] = 1;
 163     src[Y] = 0;
 164     src[Z] = 0;
 165
 166     float matrix[3][3];
 167     initYAxisRotation(matrix, 180);
 168     rotateVector(matrix, src, test);
 169     initZAxisRotation(matrix, angle);
 170     rotateVector(matrix, test, test);
 171
 172     applySensorAlignment(src, src, rotation);
 173
 174     EXPECT_EQ(test[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 175     EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 176     EXPECT_EQ(test[Z], src[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 177
 178     // unit vector along y-axis
 179     src[X] = 0;
 180     src[Y] = 1;
 181     src[Z] = 0;
 182
 183     initYAxisRotation(matrix, 180);
 184     rotateVector(matrix, src, test);
 185     initZAxisRotation(matrix, angle);
 186     rotateVector(matrix, test, test);
 187
 188     applySensorAlignment(src, src, rotation);
 189
 190     EXPECT_EQ(test[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 191     EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 192     EXPECT_EQ(test[Z], src[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 193
 194     // unit vector along z-axis
 195     src[X] = 0;
 196     src[Y] = 0;
 197     src[Z] = 1;
 198
 199     initYAxisRotation(matrix, 180);
 200     rotateVector(matrix, src, test);
 201     initZAxisRotation(matrix, angle);
 202     rotateVector(matrix, test, test);
 203
 204     applySensorAlignment(src, src, rotation);
 205
 206     EXPECT_EQ(test[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
 207     EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 208     EXPECT_EQ(test[Z], src[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 209
 210      // random vector to test
 211     src[X] = rand() % 5;
 212     src[Y] = rand() % 5;
 213     src[Z] = rand() % 5;
 214
 215     initYAxisRotation(matrix, 180);
 216     rotateVector(matrix, src, test);
 217     initZAxisRotation(matrix, angle);
 218     rotateVector(matrix, test, test);
 219
 220     applySensorAlignment(src, src, rotation);
 221
 222     EXPECT_EQ(test[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
 223     EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
 224     EXPECT_EQ(test[Z], src[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
 225 }
 226
 227
 228 TEST(AlignSensorTest, ClockwiseZeroDegrees)
 229 {
 230     srand(time(NULL));
 231     testCW(CW0_DEG, 0);
 232 }
 233
 234 TEST(AlignSensorTest, ClockwiseNinetyDegrees)
 235 {
 236     testCW(CW90_DEG, 90);
 237 }
 238
 239 TEST(AlignSensorTest, ClockwiseOneEightyDegrees)
 240 {
 241     testCW(CW180_DEG, 180);
 242 }
 243
 244 TEST(AlignSensorTest, ClockwiseTwoSeventyDegrees)
 245 {
 246     testCW(CW270_DEG, 270);
 247 }
 248
 249 TEST(AlignSensorTest, ClockwiseZeroDegreesFlip)
 250 {
 251     testCWFlip(CW0_DEG_FLIP, 0);
 252 }
 253
 254 TEST(AlignSensorTest, ClockwiseNinetyDegreesFlip)
 255 {
 256     testCWFlip(CW90_DEG_FLIP, 90);
 257 }
 258
 259 TEST(AlignSensorTest, ClockwiseOneEightyDegreesFlip)
 260 {
 261     testCWFlip(CW180_DEG_FLIP, 180);
 262 }
 263
 264 TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
 265 {
 266     testCWFlip(CW270_DEG_FLIP, 270);
 267 }
 268