Merge pull request #10550 from etracer65/fix_serialportidentifier_enum
[betaflight.git] / src / test / unit / alignsensor_unittest.cc
blobe3a093d3f384909492171b4a96f8d9e6a4227bb5
1 /*
2 * This file is part of Cleanflight.
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.
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.
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/>.
18 #include "math.h"
19 #include "stdint.h"
20 #include "time.h"
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"
31 #include "gtest/gtest.h"
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.
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.
43 #define DEG2RAD 0.01745329251
45 static void rotateVector(int32_t mat[3][3], float vec[3], float *out)
47 float tmp[3];
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];
56 out[0]=tmp[0];
57 out[1]=tmp[1];
58 out[2]=tmp[2];
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 //}
75 static void initYAxisRotation(int32_t mat[][3], int32_t angle)
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);
88 static void initZAxisRotation(int32_t mat[][3], int32_t angle)
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;
101 #define TOL 1e-5 // TOLERANCE
103 static void alignSensorViaMatrixFromRotation(float *dest, sensor_align_e alignment)
105 fp_rotationMatrix_t sensorRotationMatrix;
107 sensorAlignment_t sensorAlignment;
109 buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
111 buildRotationMatrixFromAlignment(&sensorAlignment, &sensorRotationMatrix);
113 alignSensorViaMatrix(dest, &sensorRotationMatrix);
116 static void testCW(sensor_align_e rotation, int32_t angle)
118 float src[XYZ_AXIS_COUNT];
119 float test[XYZ_AXIS_COUNT];
121 // unit vector along x-axis
122 src[X] = 1;
123 src[Y] = 0;
124 src[Z] = 0;
126 int32_t matrix[3][3];
127 initZAxisRotation(matrix, angle);
128 rotateVector(matrix, src, test);
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];
135 // unit vector along y-axis
136 src[X] = 0;
137 src[Y] = 1;
138 src[Z] = 0;
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];
146 // unit vector along z-axis
147 src[X] = 0;
148 src[Y] = 0;
149 src[Z] = 1;
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];
157 // random vector to test
158 src[X] = rand() % 5;
159 src[Y] = rand() % 5;
160 src[Z] = rand() % 5;
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];
170 * Since the order of flip and rotation matters, these tests make the
171 * assumption that the 'flip' occurs first, followed by clockwise rotation
173 static void testCWFlip(sensor_align_e rotation, int32_t angle)
175 float src[XYZ_AXIS_COUNT];
176 float test[XYZ_AXIS_COUNT];
178 // unit vector along x-axis
179 src[X] = 1;
180 src[Y] = 0;
181 src[Z] = 0;
183 int32_t matrix[3][3];
184 initYAxisRotation(matrix, 180);
185 rotateVector(matrix, src, test);
186 initZAxisRotation(matrix, angle);
187 rotateVector(matrix, test, test);
189 alignSensorViaMatrixFromRotation(src, rotation);
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];
195 // unit vector along y-axis
196 src[X] = 0;
197 src[Y] = 1;
198 src[Z] = 0;
200 initYAxisRotation(matrix, 180);
201 rotateVector(matrix, src, test);
202 initZAxisRotation(matrix, angle);
203 rotateVector(matrix, test, test);
205 alignSensorViaMatrixFromRotation(src, rotation);
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];
211 // unit vector along z-axis
212 src[X] = 0;
213 src[Y] = 0;
214 src[Z] = 1;
216 initYAxisRotation(matrix, 180);
217 rotateVector(matrix, src, test);
218 initZAxisRotation(matrix, angle);
219 rotateVector(matrix, test, test);
221 alignSensorViaMatrixFromRotation(src, rotation);
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];
227 // random vector to test
228 src[X] = rand() % 5;
229 src[Y] = rand() % 5;
230 src[Z] = rand() % 5;
232 initYAxisRotation(matrix, 180);
233 rotateVector(matrix, src, test);
234 initZAxisRotation(matrix, angle);
235 rotateVector(matrix, test, test);
237 alignSensorViaMatrixFromRotation(src, rotation);
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];
245 TEST(AlignSensorTest, ClockwiseZeroDegrees)
247 srand(time(NULL));
248 testCW(CW0_DEG, 0);
251 TEST(AlignSensorTest, ClockwiseNinetyDegrees)
253 testCW(CW90_DEG, 90);
256 TEST(AlignSensorTest, ClockwiseOneEightyDegrees)
258 testCW(CW180_DEG, 180);
261 TEST(AlignSensorTest, ClockwiseTwoSeventyDegrees)
263 testCW(CW270_DEG, 270);
266 TEST(AlignSensorTest, ClockwiseZeroDegreesFlip)
268 testCWFlip(CW0_DEG_FLIP, 0);
271 TEST(AlignSensorTest, ClockwiseNinetyDegreesFlip)
273 testCWFlip(CW90_DEG_FLIP, 90);
276 TEST(AlignSensorTest, ClockwiseOneEightyDegreesFlip)
278 testCWFlip(CW180_DEG_FLIP, 180);
281 TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
283 testCWFlip(CW270_DEG_FLIP, 270);
286 static void testBuildAlignmentWithStandardAlignment(sensor_align_e alignment, sensorAlignment_t expectedSensorAlignment)
288 sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(6, 6, 6);
290 buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
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;
297 TEST(AlignSensorTest, AttemptBuildAlignmentWithStandardAlignment)
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);
309 TEST(AlignSensorTest, AttemptBuildAlignmentFromCustomAlignment)
311 sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
313 buildAlignmentFromStandardAlignment(&sensorAlignment, ALIGN_CUSTOM);
315 sensorAlignment_t expectedSensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
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.";
322 TEST(AlignSensorTest, AttemptBuildAlignmentFromDefaultAlignment)
324 sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
326 buildAlignmentFromStandardAlignment(&sensorAlignment, ALIGN_DEFAULT);
328 sensorAlignment_t expectedSensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
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.";
335 TEST(AlignSensorTest, AlignmentBitmasks)
337 uint8_t bits;
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));
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));
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));
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));
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));
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));
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));
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));
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));
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));
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));
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));
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));
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));
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));
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));