|
0
|
1 #include "compass.h"
|
|
|
2
|
|
|
3 static unsigned short int compass_N;
|
|
|
4
|
|
|
5 static float Su, Sv, Sw;
|
|
|
6 static float Suu, Svv, Sww, Suv, Suw, Svw;
|
|
|
7 static float Suuu, Svvv, Swww;
|
|
|
8 static float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv;
|
|
|
9
|
|
|
10 //////////////////////////////////////////////////////////////////////////////
|
|
|
11 // mH: Crude work-around, needs to be made right
|
|
|
12 #ifndef UNIX
|
|
|
13 # pragma udata overlay bank8=0x800
|
|
|
14 static char C_STACK[256]; // Overlay C-code data stack here.
|
|
|
15 # define RESET_C_STACK \
|
|
|
16 _asm \
|
|
|
17 LFSR 1, 0x800 \
|
|
|
18 LFSR 2, 0x800 \
|
|
|
19 _endasm
|
|
|
20 # pragma udata overlay bank9_compass
|
|
|
21 #else
|
|
|
22 # define RESET_C_STACK
|
|
|
23 #endif
|
|
|
24
|
|
|
25 //////////////////////////////////////////////////////////////////////////////
|
|
|
26
|
|
|
27 void compass_reset_calibration()
|
|
|
28 {
|
|
|
29 RESET_C_STACK;
|
|
|
30
|
|
|
31 compass_N = 0;
|
|
|
32 Su = Sv = Sw = 0.0;
|
|
|
33 Suu = Svv = Sww = Suv = Suw = Svw = 0.0;
|
|
|
34 Suuu = Svvv = Swww = 0.0;
|
|
|
35 Suuv = Suuw = Svvu = Svvw = Swwu = Swwv = 0.0;
|
|
|
36 }
|
|
|
37
|
|
|
38 void compass_add_calibration()
|
|
|
39 {
|
|
|
40 OVERLAY float u, v, w;
|
|
|
41 RESET_C_STACK;
|
|
|
42
|
|
|
43 u = (compass_DX_f - compass_CX_f) / 32768.0f;
|
|
|
44 v = (compass_DY_f - compass_CY_f) / 32768.0f;
|
|
|
45 w = (compass_DZ_f - compass_CZ_f) / 32768.0f;
|
|
|
46
|
|
|
47 compass_N++;
|
|
|
48 Su += u;
|
|
|
49 Sv += v;
|
|
|
50 Sw += w;
|
|
|
51 Suv += u*v;
|
|
|
52 Suw += u*w;
|
|
|
53 Svw += v*w;
|
|
|
54 Suu += u*u;
|
|
|
55 Suuu += u*u*u;
|
|
|
56 Suuv += v*u*u;
|
|
|
57 Suuw += w*u*u;
|
|
|
58 Svv += v*v;
|
|
|
59 Svvv += v*v*v;
|
|
|
60 Svvu += u*v*v;
|
|
|
61 Svvw += w*v*v;
|
|
|
62 Sww += w*w;
|
|
|
63 Swww += w*w*w;
|
|
|
64 Swwu += u*w*w;
|
|
|
65 Swwv += v*w*w;
|
|
|
66 }
|
|
|
67
|
|
|
68 //////////////////////////////////////////////////////////////////////////////
|
|
|
69
|
|
|
70 void compass_solve_calibration()
|
|
|
71 {
|
|
|
72 OVERLAY float yu, yv, yw;
|
|
|
73 OVERLAY float delta;
|
|
|
74 OVERLAY float uc, vc, wc;
|
|
|
75 RESET_C_STACK;
|
|
|
76
|
|
|
77 //---- Normalize partial sums --------------------------------------------
|
|
|
78 //
|
|
|
79 // u, v, w should be centered on the mean value um, vm, wm:
|
|
|
80 // x = u + um, with um = Sx/N
|
|
|
81 //
|
|
|
82 // So:
|
|
|
83 // (u + um)**2 = u**2 + 2u*um + um**2
|
|
|
84 // Su = 0, um = Sx/N
|
|
|
85 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N
|
|
|
86 // Suu = Sxx - Sx**2/N
|
|
|
87 yu = Su/compass_N;
|
|
|
88 yv = Sv/compass_N;
|
|
|
89 yw = Sw/compass_N;
|
|
|
90
|
|
|
91 Suu -= Su*yu;
|
|
|
92 Svv -= Sv*yv;
|
|
|
93 Sww -= Sw*yw;
|
|
|
94
|
|
|
95 // (u + um)(v + vm) = uv + u vm + v um + um vm
|
|
|
96 // Sxy = Suv + N * um vm
|
|
|
97 // Suv = Sxy - N * (Sx/N)(Sy/N);
|
|
|
98 Suv -= Su*yv;
|
|
|
99 Suw -= Su*yw;
|
|
|
100 Svw -= Sv*yw;
|
|
|
101
|
|
|
102 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
|
|
|
103 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
|
|
|
104 // Su = 0, um = Sx/N:
|
|
|
105 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
|
|
|
106 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
|
|
|
107
|
|
|
108 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
|
|
|
109 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
|
|
|
110 //
|
|
|
111 // Su = 0, Sv = 0, vm = Sy/N:
|
|
|
112 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
|
|
|
113 //
|
|
|
114 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
|
|
|
115 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
|
|
|
116 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
|
|
|
117 Suuu -= (3*Suu + Su*yu)*yu;
|
|
|
118 Suuv -= (Suu + Su*yu)*yv + 2*Suv*yu;
|
|
|
119 Suuw -= (Suu + Su*yu)*yw + 2*Suw*yu;
|
|
|
120
|
|
|
121 Svvu -= (Svv + Sv*yv)*yu + 2*Suv*yv;
|
|
|
122 Svvv -= (3*Svv + Sv*yv)*yv;
|
|
|
123 Svvw -= (Svv + Sv*yv)*yw + 2*Svw*yv;
|
|
|
124
|
|
|
125 Swwu -= (Sww + Sw*yw)*yu + 2*Suw*yw;
|
|
|
126 Swwv -= (Sww + Sw*yw)*yv + 2*Svw*yw;
|
|
|
127 Swww -= (3*Sww + Sw*yw)*yw;
|
|
|
128
|
|
|
129 //---- Solve the system --------------------------------------------------
|
|
|
130 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
|
|
|
131 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
|
|
|
132 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
|
|
|
133 // Note this is symetric, with a positiv diagonal, hence
|
|
|
134 // it always have a uniq solution.
|
|
|
135 yu = 0.5f * (Suuu + Svvu + Swwu);
|
|
|
136 yv = 0.5f * (Suuv + Svvv + Swwv);
|
|
|
137 yw = 0.5f * (Suuw + Svvw + Swww);
|
|
|
138 delta = Suu * (Svv * Sww - Svw * Svw)
|
|
|
139 - Suv * (Suv * Sww - Svw * Suw)
|
|
|
140 + Suw * (Suv * Svw - Svv * Suw);
|
|
|
141
|
|
|
142 uc = (yu * (Svv * Sww - Svw * Svw)
|
|
|
143 - yv * (Suv * Sww - Svw * Suw)
|
|
|
144 + yw * (Suv * Svw - Svv * Suw) )/delta;
|
|
|
145 vc = (Suu * ( yv * Sww - yw * Svw)
|
|
|
146 - Suv * ( yu * Sww - yw * Suw)
|
|
|
147 + Suw * ( yu * Svw - yv * Suw) )/delta;
|
|
|
148 wc = (Suu * (Svv * yw - Svw * yv )
|
|
|
149 - Suv * (Suv * yw - Svw * yu )
|
|
|
150 + Suw * (Suv * yv - Svv * yu ) )/delta;
|
|
|
151
|
|
|
152 // Back to uncentered coordinates:
|
|
|
153 // xc = um + uc
|
|
|
154 uc = Su/compass_N + compass_CX_f/32768.0f + uc;
|
|
|
155 vc = Sv/compass_N + compass_CY_f/32768.0f + vc;
|
|
|
156 wc = Sw/compass_N + compass_CZ_f/32768.0f + wc;
|
|
|
157
|
|
|
158 // Then save the new calibrated center:
|
|
|
159 compass_CX_f = (short)(32768 * uc);
|
|
|
160 compass_CY_f = (short)(32768 * vc);
|
|
|
161 compass_CZ_f = (short)(32768 * wc);
|
|
|
162 }
|
|
|
163
|
|
|
164 ////////////////////////////// TEST CODE /////////////////////////////////////
|
|
|
165
|
|
|
166 #ifdef TEST_COMPASS_CALIBRATION
|
|
|
167
|
|
|
168 #include <QtDebug>
|
|
|
169 #include <stdio.h>
|
|
|
170
|
|
|
171 #include <math.h>
|
|
|
172 #include <stdlib.h>
|
|
|
173
|
|
|
174 short compass_DX_f, compass_DY_f, compass_DZ_f;
|
|
|
175 short compass_CX_f, compass_CY_f, compass_CZ_f;
|
|
|
176
|
|
|
177 inline float uniform() {
|
|
|
178 return (rand() & 0xFFFF) / 65536.0f;
|
|
|
179 }
|
|
|
180 inline float sqr(float x) {
|
|
|
181 return x*x;
|
|
|
182 }
|
|
|
183
|
|
|
184 static const float radius = 0.21f;
|
|
|
185 static const float cx = 0.79f, cy = -0.46f, cz = 0.24f;
|
|
|
186 // const float cx = 0, cy = 0, cz = 0;
|
|
|
187
|
|
|
188 void check_compass_calib()
|
|
|
189 {
|
|
|
190
|
|
|
191 // Starts with no calibration at all:
|
|
|
192 compass_CX_f = compass_CY_f = compass_CZ_f = 0;
|
|
|
193
|
|
|
194 // Try 10 recalibration passes:
|
|
|
195 for(int p=0; p<10; ++p)
|
|
|
196 {
|
|
|
197 compass_reset_calibration();
|
|
|
198
|
|
|
199 //---- Generates random points on a sphere -------------------------------
|
|
|
200 // of radius,center (cx, cy, cz):
|
|
|
201 for(int i=0; i<100; ++i)
|
|
|
202 {
|
|
|
203 float theta = uniform()*360.0f;
|
|
|
204 float phi = uniform()*180.0f - 90.0f;
|
|
|
205
|
|
|
206 float x = cx + radius * cosf(phi)*cosf(theta);
|
|
|
207 float y = cy + radius * cosf(phi)*sinf(theta);
|
|
|
208 float z = cz + radius * sinf(phi);
|
|
|
209
|
|
|
210 compass_DX_f = short(32768 * x);
|
|
|
211 compass_DY_f = short(32768 * y);
|
|
|
212 compass_DZ_f = short(32768 * z);
|
|
|
213 compass_add_calibration();
|
|
|
214 }
|
|
|
215
|
|
|
216 compass_solve_calibration();
|
|
|
217 qDebug() << "Center ="
|
|
|
218 << compass_CX_f/32768.0f
|
|
|
219 << compass_CY_f/32768.0f
|
|
|
220 << compass_CZ_f/32768.0f;
|
|
|
221
|
|
|
222 float r2 = sqr(compass_CX_f/32768.0f - cx)
|
|
|
223 + sqr(compass_CY_f/32768.0f - cy)
|
|
|
224 + sqr(compass_CZ_f/32768.0f - cz);
|
|
|
225 if( r2 > 0.01f*0.01f )
|
|
|
226 qWarning() << " calibration error: " << sqrtf(r2);
|
|
|
227 }
|
|
|
228 }
|
|
|
229 #endif // TEST_COMPASS_CALIBRATION
|
