Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1702 | - | 1 | /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
2 | // |
||
3 | // Unit tests for the AP_Math euler code |
||
4 | // |
||
5 | |||
6 | #include <FastSerial.h> |
||
7 | #include <AP_Common.h> |
||
8 | #include <AP_Math.h> |
||
9 | |||
10 | FastSerialPort(Serial, 0); |
||
11 | |||
12 | #ifdef DESKTOP_BUILD |
||
13 | // all of this is needed to build with SITL |
||
14 | #include <DataFlash.h> |
||
15 | #include <APM_RC.h> |
||
16 | #include <GCS_MAVLink.h> |
||
17 | #include <Arduino_Mega_ISR_Registry.h> |
||
18 | #include <AP_PeriodicProcess.h> |
||
19 | #include <AP_ADC.h> |
||
20 | #include <SPI.h> |
||
21 | #include <I2C.h> |
||
22 | #include <AP_Baro.h> |
||
23 | #include <AP_Compass.h> |
||
24 | #include <AP_GPS.h> |
||
25 | #include <Filter.h> |
||
26 | Arduino_Mega_ISR_Registry isr_registry; |
||
27 | AP_Baro_BMP085_HIL barometer; |
||
28 | AP_Compass_HIL compass; |
||
29 | #endif |
||
30 | |||
31 | #include <AP_Declination.h> |
||
32 | |||
33 | |||
34 | static float rad_diff(float rad1, float rad2) |
||
35 | { |
||
36 | float diff = rad1 - rad2; |
||
37 | if (diff > PI) { |
||
38 | diff -= 2*PI; |
||
39 | } |
||
40 | if (diff < -PI) { |
||
41 | diff += 2*PI; |
||
42 | } |
||
43 | return fabs(diff); |
||
44 | } |
||
45 | |||
46 | static void check_result(float roll, float pitch, float yaw, |
||
47 | float roll2, float pitch2, float yaw2) |
||
48 | { |
||
49 | if (isnan(roll2) || |
||
50 | isnan(pitch2) || |
||
51 | isnan(yaw2)) { |
||
52 | Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n", |
||
53 | roll, pitch, yaw); |
||
54 | } |
||
55 | |||
56 | if (rad_diff(roll2,roll) > ToRad(179)) { |
||
57 | // reverse all 3 |
||
58 | roll2 += fmod(roll2+PI, 2*PI); |
||
59 | pitch2 += fmod(pitch2+PI, 2*PI); |
||
60 | yaw2 += fmod(yaw2+PI, 2*PI); |
||
61 | } |
||
62 | |||
63 | if (rad_diff(roll2,roll) > 0.01 || |
||
64 | rad_diff(pitch2, pitch) > 0.01 || |
||
65 | rad_diff(yaw2, yaw) > 0.01) { |
||
66 | if (pitch >= PI/2 || |
||
67 | pitch <= -PI/2 || |
||
68 | ToDeg(rad_diff(pitch, PI/2)) < 1 || |
||
69 | ToDeg(rad_diff(pitch, -PI/2)) < 1) { |
||
70 | // we expect breakdown at these poles |
||
71 | Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", |
||
72 | ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); |
||
73 | } else { |
||
74 | Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", |
||
75 | ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); |
||
76 | } |
||
77 | } |
||
78 | } |
||
79 | |||
80 | static void test_euler(float roll, float pitch, float yaw) |
||
81 | { |
||
82 | Matrix3f m; |
||
83 | float roll2, pitch2, yaw2; |
||
84 | |||
85 | m.from_euler(roll, pitch, yaw); |
||
86 | m.to_euler(&roll2, &pitch2, &yaw2); |
||
87 | check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
||
88 | } |
||
89 | |||
90 | #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) |
||
91 | |||
92 | static const float angles[] = { 0, PI/8, PI/4, PI/2, PI, |
||
93 | -PI/8, -PI/4, -PI/2, -PI}; |
||
94 | |||
95 | void test_matrix_eulers(void) |
||
96 | { |
||
97 | uint8_t i, j, k; |
||
98 | uint8_t N = ARRAY_LENGTH(angles); |
||
99 | |||
100 | Serial.println("rotation matrix unit tests\n"); |
||
101 | |||
102 | for (i=0; i<N; i++) |
||
103 | for (j=0; j<N; j++) |
||
104 | for (k=0; k<N; k++) |
||
105 | test_euler(angles[i], angles[j], angles[k]); |
||
106 | |||
107 | Serial.println("tests done\n"); |
||
108 | } |
||
109 | |||
110 | static void test_quaternion(float roll, float pitch, float yaw) |
||
111 | { |
||
112 | Quaternion q; |
||
113 | float roll2, pitch2, yaw2; |
||
114 | |||
115 | q.from_euler(roll, pitch, yaw); |
||
116 | q.to_euler(&roll2, &pitch2, &yaw2); |
||
117 | check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
||
118 | } |
||
119 | |||
120 | void test_quaternion_eulers(void) |
||
121 | { |
||
122 | uint8_t i, j, k; |
||
123 | uint8_t N = ARRAY_LENGTH(angles); |
||
124 | |||
125 | Serial.println("quaternion unit tests\n"); |
||
126 | |||
127 | test_quaternion(PI/4, 0, 0); |
||
128 | test_quaternion(0, PI/4, 0); |
||
129 | test_quaternion(0, 0, PI/4); |
||
130 | test_quaternion(-PI/4, 0, 0); |
||
131 | test_quaternion(0, -PI/4, 0); |
||
132 | test_quaternion(0, 0, -PI/4); |
||
133 | test_quaternion(-PI/4, 1, 1); |
||
134 | test_quaternion(1, -PI/4, 1); |
||
135 | test_quaternion(1, 1, -PI/4); |
||
136 | |||
137 | test_quaternion(ToRad(89), 0, 0.1); |
||
138 | test_quaternion(0, ToRad(89), 0.1); |
||
139 | test_quaternion(0.1, 0, ToRad(89)); |
||
140 | |||
141 | test_quaternion(ToRad(91), 0, 0.1); |
||
142 | test_quaternion(0, ToRad(91), 0.1); |
||
143 | test_quaternion(0.1, 0, ToRad(91)); |
||
144 | |||
145 | for (i=0; i<N; i++) |
||
146 | for (j=0; j<N; j++) |
||
147 | for (k=0; k<N; k++) |
||
148 | test_quaternion(angles[i], angles[j], angles[k]); |
||
149 | |||
150 | Serial.println("tests done\n"); |
||
151 | } |
||
152 | |||
153 | |||
154 | static void test_conversion(float roll, float pitch, float yaw) |
||
155 | { |
||
156 | Quaternion q; |
||
157 | Matrix3f m, m2; |
||
158 | |||
159 | float roll2, pitch2, yaw2; |
||
160 | float roll3, pitch3, yaw3; |
||
161 | |||
162 | q.from_euler(roll, pitch, yaw); |
||
163 | q.to_euler(&roll2, &pitch2, &yaw2); |
||
164 | check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
||
165 | |||
166 | q.rotation_matrix(m); |
||
167 | m.to_euler(&roll2, &pitch2, &yaw2); |
||
168 | |||
169 | m2.from_euler(roll, pitch, yaw); |
||
170 | m2.to_euler(&roll3, &pitch3, &yaw3); |
||
171 | if (m.is_nan()) { |
||
172 | Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n", |
||
173 | roll, pitch, yaw); |
||
174 | } |
||
175 | |||
176 | check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
||
177 | check_result(roll, pitch, yaw, roll3, pitch3, yaw3); |
||
178 | } |
||
179 | |||
180 | void test_conversions(void) |
||
181 | { |
||
182 | uint8_t i, j, k; |
||
183 | uint8_t N = ARRAY_LENGTH(angles); |
||
184 | |||
185 | Serial.println("matrix/quaternion tests\n"); |
||
186 | |||
187 | test_conversion(1, 1.1, 1.2); |
||
188 | test_conversion(1, -1.1, 1.2); |
||
189 | test_conversion(1, -1.1, -1.2); |
||
190 | test_conversion(-1, 1.1, -1.2); |
||
191 | test_conversion(-1, 1.1, 1.2); |
||
192 | |||
193 | for (i=0; i<N; i++) |
||
194 | for (j=0; j<N; j++) |
||
195 | for (k=0; k<N; k++) |
||
196 | test_conversion(angles[i], angles[j], angles[k]); |
||
197 | |||
198 | Serial.println("tests done\n"); |
||
199 | } |
||
200 | |||
201 | void test_frame_transforms(void) |
||
202 | { |
||
203 | Vector3f v, v2; |
||
204 | Quaternion q; |
||
205 | Matrix3f m; |
||
206 | |||
207 | Serial.println("frame transform tests\n"); |
||
208 | |||
209 | q.from_euler(ToRad(90), 0, 0); |
||
210 | v2 = v = Vector3f(0, 0, 1); |
||
211 | q.earth_to_body(v2); |
||
212 | printf("%f %f %f\n", v2.x, v2.y, v2.z); |
||
213 | } |
||
214 | |||
215 | // generate a random float between -1 and 1 |
||
216 | static float rand_num(void) |
||
217 | { |
||
218 | float ret = ((unsigned)random()) % 2000000; |
||
219 | return (ret - 1.0e6) / 1.0e6; |
||
220 | } |
||
221 | |||
222 | void test_matrix_rotate(void) |
||
223 | { |
||
224 | Matrix3f m1, m2, diff; |
||
225 | Vector3f r; |
||
226 | |||
227 | m1.identity(); |
||
228 | m2.identity(); |
||
229 | r.x = rand_num(); |
||
230 | r.y = rand_num(); |
||
231 | r.z = rand_num(); |
||
232 | |||
233 | for (uint16_t i = 0; i<1000; i++) { |
||
234 | // old method |
||
235 | Matrix3f temp_matrix; |
||
236 | temp_matrix.a.x = 0; |
||
237 | temp_matrix.a.y = -r.z; |
||
238 | temp_matrix.a.z = r.y; |
||
239 | temp_matrix.b.x = r.z; |
||
240 | temp_matrix.b.y = 0; |
||
241 | temp_matrix.b.z = -r.x; |
||
242 | temp_matrix.c.x = -r.y; |
||
243 | temp_matrix.c.y = r.x; |
||
244 | temp_matrix.c.z = 0; |
||
245 | temp_matrix = m1 * temp_matrix; |
||
246 | m1 += temp_matrix; |
||
247 | |||
248 | // new method |
||
249 | m2.rotate(r); |
||
250 | |||
251 | // check they behave in the same way |
||
252 | diff = m1 - m2; |
||
253 | float err = diff.a.length() + diff.b.length() + diff.c.length(); |
||
254 | |||
255 | if (err > 0) { |
||
256 | Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err); |
||
257 | } |
||
258 | } |
||
259 | } |
||
260 | |||
261 | /* |
||
262 | euler angle tests |
||
263 | */ |
||
264 | void setup(void) |
||
265 | { |
||
266 | Serial.begin(115200); |
||
267 | Serial.println("euler unit tests\n"); |
||
268 | |||
269 | test_conversion(0, PI, 0); |
||
270 | |||
271 | test_frame_transforms(); |
||
272 | test_conversions(); |
||
273 | test_quaternion_eulers(); |
||
274 | test_matrix_eulers(); |
||
275 | test_matrix_rotate(); |
||
276 | } |
||
277 | |||
278 | void |
||
279 | loop(void) |
||
280 | { |
||
281 | } |