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