Mercurial > public > hwos_code

comparison src/compass_calib.c @ 0:11d4fc797f74

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
init
author heinrichsweikamp
date Wed, 24 Apr 2013 19:22:45 +0200
parents
children a4bff632e97b
comparison
equal deleted inserted replaced
-1:000000000000 0:11d4fc797f74
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