282
|
1 //////////////////////////////////////////////////////////////////////////////
|
|
2 /// compass_test.cpp
|
|
3 /// Unit test for compass calibration.
|
|
4 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
|
|
5 //////////////////////////////////////////////////////////////////////////////
|
|
6 // HISTORY
|
|
7 // 2015-05-23 jDG: Rewrite compass testing, to allow reducing code size.
|
|
8
|
|
9 extern "C" {
|
|
10 # include "compass.h"
|
|
11 }
|
|
12
|
|
13 #include <gtest/gtest.h>
|
|
14
|
|
15 #include <math.h>
|
|
16 #include <iostream>
|
|
17
|
|
18 //////////////////////////////////////////////////////////////////////////////
|
|
19
|
|
20 inline float uniform() {
|
|
21 return (rand() & 0xFFFF) / 65536.0f;
|
|
22 }
|
|
23 inline float sqr(float x) {
|
|
24 return x*x;
|
|
25 }
|
|
26
|
|
27 static float radius = 0.21f;
|
|
28 static float cx = 0, cy = 0, cz = 0;
|
|
29
|
|
30 //////////////////////////////////////////////////////////////////////////////
|
|
31
|
|
32 static void check_calib()
|
|
33 {
|
|
34 compass_reset_calibration();
|
|
35
|
|
36 //---- Generates random points on a sphere -------------------------------
|
|
37 // of radius,center (cx, cy, cz):
|
|
38 for(int i=0; i<100; ++i)
|
|
39 {
|
|
40 float theta = uniform()*360.0f;
|
|
41 float phi = uniform()*180.0f - 90.0f;
|
|
42
|
|
43 float x = cx + radius * cosf(phi)*cosf(theta);
|
|
44 float y = cy + radius * cosf(phi)*sinf(theta);
|
|
45 float z = cz + radius * sinf(phi);
|
|
46
|
|
47 compass_DX_f = short(32768 * x);
|
|
48 compass_DY_f = short(32768 * y);
|
|
49 compass_DZ_f = short(32768 * z);
|
|
50 compass_add_calibration();
|
|
51 }
|
|
52
|
|
53 compass_solve_calibration();
|
|
54 float r2 = sqr(compass_CX_f/32768.0f - cx)
|
|
55 + sqr(compass_CY_f/32768.0f - cy)
|
|
56 + sqr(compass_CZ_f/32768.0f - cz);
|
|
57
|
|
58 // Calibration error less than 2 bits:
|
285
|
59 EXPECT_NEAR(0, sqrtf(r2), 4.0f/32768.0f)
|
282
|
60 << "Center at (" << compass_CX_f/32768.0f << ", "
|
|
61 << compass_CY_f/32768.0f << ", "
|
|
62 << compass_CZ_f/32768.0f << ")."
|
|
63 << " Error = " << sqrtf(r2);
|
|
64 }
|
|
65
|
|
66 //////////////////////////////////////////////////////////////////////////////
|
|
67
|
|
68 TEST(compass, calibration_centered)
|
|
69 {
|
|
70 compass_CX_f = compass_CY_f = compass_CZ_f = 0;
|
|
71
|
|
72 // Half-unit, centered, sphere:
|
|
73 radius = 0.5f;
|
|
74
|
|
75 // Try 10 recalibration passes:
|
|
76 for(int p=0; p<10; ++p)
|
|
77 check_calib();
|
|
78 }
|
|
79
|
|
80 //////////////////////////////////////////////////////////////////////////////
|
|
81
|
|
82 TEST(compass, calibration_near_centered)
|
|
83 {
|
|
84 // Put magnetic center elsewhere, but keep position+radius < 1.0, to
|
|
85 // avoid Q15 overflow...
|
|
86 radius = 0.21f;
|
|
87 cx = 0.019f, cy = -0.026f, cz = 0.004f;
|
|
88
|
|
89 // Try 10 recalibration passes:
|
|
90 for(int p=0; p<10; ++p)
|
|
91 check_calib();
|
|
92 }
|
|
93
|
|
94 //////////////////////////////////////////////////////////////////////////////
|
|
95
|
|
96 TEST(compass, calibration_far_centered)
|
|
97 {
|
|
98 // Put magnetic center elsewhere, but keep position+radius < 1.0, to
|
|
99 // avoid Q15 overflow...
|
|
100 radius = 0.21f;
|
|
101 cx = -0.79f, cy = 0.79f, cz = 0.79f;
|
|
102
|
|
103 // Try 10 recalibration passes:
|
|
104 for(int p=0; p<10; ++p)
|
|
105 check_calib();
|
|
106 }
|
|
107
|
|
108 //////////////////////////////////////////////////////////////////////////////
|
|
109
|
|
110 TEST(compass, calibration_small_centered_signal)
|
|
111 {
|
|
112 // use a very very small magnetic signal, centered:
|
|
113 radius = 0.001f;
|
|
114 cx = 0.000f, cy = 0.000f, cz = 0.000f;
|
|
115
|
|
116 // Try 10 recalibration passes:
|
|
117 for(int p=0; p<10; ++p)
|
|
118 check_calib();
|
|
119 }
|
|
120
|
|
121 //////////////////////////////////////////////////////////////////////////////
|
|
122
|
|
123 TEST(compass, calibration_small_off_centered_signal)
|
|
124 {
|
|
125 // Have a rather small sphere radius (20%), off-centered buy 80%
|
|
126 radius = 0.200f;
|
|
127 cx = 0.800f, cy = -0.800f, cz = 0.800f;
|
|
128
|
|
129 // Try 10 recalibration passes:
|
|
130 for(int p=0; p<10; ++p)
|
|
131 check_calib();
|
|
132 }
|