Mercurial > public > hwos_code

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