Mercurial > public > hwos_code

comparison src/Tests/compass_test.cpp @ 282:7d9edd3b8c86

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Make a more compact COMPASS calibration code (<7KB), and add more tests.
author jDG
date Fri, 22 May 2015 14:50:40 +0200
parents
children cd2320cd6f9a
comparison
equal deleted inserted replaced
281:eb758a5b44eb 282:7d9edd3b8c86
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:
59 EXPECT_NEAR(0, sqrtf(r2), 3.0f/32768.0f)
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 }