Mercurial > public > hwos_code

comparison src/compass_calib.c @ 623:c40025d8e750

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
3.03 beta released
author heinrichsweikamp
date Mon, 03 Jun 2019 14:01:48 +0200
parents ca4556fb60b9
children cd58f7fc86db
comparison
equal deleted inserted replaced
622:02d1386429a6 623:c40025d8e750
1 ////////////////////////////////////////////////////////////////////////////// 1 /////////////////////////////////////////////////////////////////////////////
2 /// compass_calib.c 2 //
3 /// Calibrate hard-iron for magnetic compass measurements. 3 // compass_calib.c next generation V3.03.4
4 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved. 4 //
5 ////////////////////////////////////////////////////////////////////////////// 5 // Calibrate hard-iron for magnetic compass measurements.
6 // Copyright (c) 2012-2019, JD Gascuel, HeinrichsWeikamp, all rights reserved.
7 //
8 //////////////////////////////////////////////////////////////////////////////
9
6 // 2015-05-22 [jDG] Make a smaller calibration code (15.6 --> 6.7 KB). 10 // 2015-05-22 [jDG] Make a smaller calibration code (15.6 --> 6.7 KB).
7 11 // 2019-05-14 [rl] make it even smaller again by another 2.000 byte
12
13 #include "configuration.inc"
8 #include "compass.h" 14 #include "compass.h"
9 15
10 ////////////////////////////////////////////////////////////////////////////// 16
17 #ifdef _compass
18
19
20 //////////////////////////////////////////////////////////////////////////////
21 //
11 // mH: Put compass data into bank 8 (stack) and bank 9 (variables) 22 // mH: Put compass data into bank 8 (stack) and bank 9 (variables)
23 // rl: could also be overlaid with p2_deco.c stack...
24 //
12 #ifndef UNIX 25 #ifndef UNIX
13 # pragma udata overlay bank8=0x800 26 # pragma udata overlay bank8=0x800
14 static char C_STACK[256]; // overlay C-code data stack here 27 static char C_STACK[256]; // overlay C-code data stack here
15 # define RESET_C_STACK \ 28 # define RESET_C_STACK \
16 _asm \ 29 _asm \
17 LFSR 1, 0x800 \ 30 LFSR 1, 0x800 \
18 LFSR 2, 0x800 \ 31 LFSR 2, 0x800 \
19 _endasm 32 _endasm
20 # pragma udata overlay bank9_compass 33 # pragma udata overlay bank9_compass
21 #else 34 #else
22 # define RESET_C_STACK 35 # define RESET_C_STACK
23 #endif 36 #endif
24 37
25 ////////////////////////////////////////////////////////////////////////////// 38 //////////////////////////////////////////////////////////////////////////////
26 39
27 static unsigned short int compass_N; 40 static unsigned short int compass_N;
28 41
29 static float Su, Sv, Sw; // first order moments 42 static float Su, Sv, Sw; // first order moments
30 static float Suu, Svv, Sww, Suv, Suw, Svw; // second order moments 43 static float Suu, Svv, Sww, Suv, Suw, Svw; // second order moments
31 static float Saa; // Suu + Svv + Sww 44 static float Saa; // Suu + Svv + Sww
32 static float Saau; // Suuu + Svvu + Swwu // third order moment 45 static float Saau; // Suuu + Svvu + Swwu third order moment
33 static float Saav; // Suuv + Svvv + Swwv 46 static float Saav; // Suuv + Svvv + Swwv third order moment
34 static float Saaw; // Suuw + Svvw + Swww 47 static float Saaw; // Suuw + Svvw + Swww third order moment
35 static float yu, yv, yw; // temp solution vector 48
36 static float uc, vc, wc; // temp sphere's center 49 static float yu, yv, yw; // temp solution vector
37 50 static float uc, vc, wc; // temp sphere's center
38 ////////////////////////////////////////////////////////////////////////////// 51
39 52 static float yh, uh, S0, S1, S2, S3; // transfer vars for compass_solve_helper()
40 void compass_reset_calibration() 53 static float discriminant, delta; // transfer vars for calc_discriminant()
54
55
56 //////////////////////////////////////////////////////////////////////////////
57
58 #ifndef UNIX
59 # pragma code compass_calib
60 #endif
61
62 //////////////////////////////////////////////////////////////////////////////
63
64 void compass_reset_calibration() // 202 byte
41 { 65 {
42 RESET_C_STACK; 66 RESET_C_STACK;
43 67
44 compass_N = 0; 68 compass_N = 0;
45 Su = Sv = Sw = 0.0; 69 Su = Sv = Sw = 0.0;
46 Suu = Svv = Sww = 0.0; 70 Suu = Svv = Sww = 0.0;
47 Suv = Suw = Svw = 0.0; 71 Suv = Suw = Svw = 0.0;
48 Saau = Saav = Saaw = 0.0; 72 Saau = Saav = Saaw = 0.0;
49 compass_CX_f = compass_CY_f = compass_CZ_f = 0; 73 compass_CX_f = compass_CY_f = compass_CZ_f = 0; // int16
50 } 74 }
51 75
52 ////////////////////////////////////////////////////////////////////////////// 76 //////////////////////////////////////////////////////////////////////////////
53 77
54 void compass_add_calibration() 78 void compass_add_calibration() // 1488 byte
55 { 79 {
80 overlay float SQR_yu, SQR_yv, SQR_yw; // squared values;
81
56 RESET_C_STACK; 82 RESET_C_STACK;
57 83
58 // get filtered/calibrated magnetic direction 84 // get filtered/calibrated magnetic direction // 276 byte
59 yu = (compass_DX_f - compass_CX_f) / 32768.0f; 85 yu = (compass_DX_f - compass_CX_f) / 32768.0f;
60 yv = (compass_DY_f - compass_CY_f) / 32768.0f; 86 yv = (compass_DY_f - compass_CY_f) / 32768.0f;
61 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f; 87 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f;
62 88
63 // add to all moments 89 // compute squared values // 156 byte
90 SQR_yu = yu*yu;
91 SQR_yv = yv*yv;
92 SQR_yw = yw*yw;
93
94 // increment count
64 compass_N++; 95 compass_N++;
65 96
66 Su += yu; 97 // add to all moments // 156 byte
67 Sv += yv; 98 Su += yu;
68 Sw += yw; 99 Sv += yv;
69 100 Sw += yw;
70 Suu += yu*yu; 101
71 Suv += yu*yv; 102 // // 156 byte
72 Suw += yu*yw; 103 Suu += SQR_yu;
73 Svv += yv*yv; 104 Svv += SQR_yv;
74 Svw += yv*yw; 105 Sww += SQR_yw;
75 Sww += yw*yw; 106
76 107 // // 312 byte
77 Saa = yu*yu + yv*yv + yw*yw; 108 Suv += yu*yv;
109 Suw += yu*yw;
110 Svw += yv*yw;
111
112 // // 104 byte
113 Saa = SQR_yu + SQR_yv + SQR_yw;
114
115
116 // // 312 byte
78 Saau += yu * Saa; 117 Saau += yu * Saa;
79 Saav += yv * Saa; 118 Saav += yv * Saa;
80 Saaw += yw * Saa; 119 Saaw += yw * Saa;
81 } 120 }
82 121
83 ////////////////////////////////////////////////////////////////////////////// 122 //////////////////////////////////////////////////////////////////////////////
84 123
85 static float compass_discriminent(PARAMETER char column) 124 static void calc_discriminant(PARAMETER char column)
86 { 125 {
87 // basic symmetric matrix 126 // basic symmetric matrix
88 OVERLAY float a = Suu, d = Suv, g = Suw; 127 overlay float a = Suu, d = Suv, g = Suw;
89 OVERLAY float b = Suv, e = Svv, h = Svw; 128 overlay float b = Suv, e = Svv, h = Svw;
90 OVERLAY float c = Suw, f = Svw, i = Sww; 129 overlay float c = Suw, f = Svw, i = Sww;
91 130
92 // substitute a column, if asked to 131 // substitute a column, if asked to
93 if( column==1 ) { a = yu; b = yv; c = yw; } 132 if( column==1 ) { a = yu; b = yv; c = yw; }
94 if( column==2 ) { d = yu; e = yv; f = yw; } 133 if( column==2 ) { d = yu; e = yv; f = yw; }
95 if( column==3 ) { g = yu; h = yv; i = yw; } 134 if( column==3 ) { g = yu; h = yv; i = yw; }
96 135
97 // do the math 136 // do the math in a couple of single terms to reduce
98 return a * (e * i - f * h) 137 // the amount of required ".tmpdata" memory
99 - b * (d * i - f * g) 138 discriminant = a * (e * i - f * h);
100 + c * (d * h - e * g); 139 discriminant -= b * (d * i - f * g);
101 } 140 discriminant += c * (d * h - e * g);
102 141
103 ////////////////////////////////////////////////////////////////////////////// 142 // apply delta if column = 1/2/3
104 143 if( column ) discriminant *= delta;
105 static float compass_dotc(PARAMETER float u, float v, float w) 144 }
106 { 145
107 return u*uc + v*vc + w*wc; 146 //////////////////////////////////////////////////////////////////////////////
108 } 147
109 148 static void compass_solve_helper(void) // 338 byte
110 ////////////////////////////////////////////////////////////////////////////// 149 {
111 150 // computes:
112 void compass_solve_calibration() 151 //
113 { 152 // yh = S0 - Saa*uh - compass_dotc(Su*uh + 2*S1, Sv*uh + 2*S2, Sw*uh + 2*S3);
114 OVERLAY float delta; 153 //
154 // with compass_dotc(u,v,w) = u*uc + v*vc + w*wc the equation becomes:
155 //
156 // yh = S0 - Saa*uh - ( (Su*uh + 2*S1)*uc + (Sv*uh + 2*S2)*vc + (Sw*uh + 2*S3)*wc )
157 //
158 // this equation is now split into several single terms
159 // to reduce the amount of required ".tmpdata" memory:
160
161 yh = S0;
162 yh -= Saa*uh;
163 yh -= (Su*uh + 2*S1) * uc;
164 yh -= (Sv*uh + 2*S2) * vc;
165 yh -= (Sw*uh + 2*S3) * wc;
166 }
167
168 //////////////////////////////////////////////////////////////////////////////
169
170 void compass_solve_calibration() // 1738 byte
171 {
172 overlay float inv_compass_N;
173
115 RESET_C_STACK; 174 RESET_C_STACK;
116 175
117 //---- Compute center of measured magnetic directions -------------------- 176 // compute center of measured magnetic directions // 200 byte
118 uc = Su/compass_N; 177 inv_compass_N = 1 / compass_N;
119 vc = Sv/compass_N; 178 uc = Su * inv_compass_N;
120 wc = Sw/compass_N; 179 vc = Sv * inv_compass_N;
121 180 wc = Sw * inv_compass_N;
122 //---- Normalize partial sums -------------------------------------------- 181
182 // normalize partial sums
123 // 183 //
124 // We measured the (u, v, w) values, and need the centered (x, y, z) ones 184 // We measured the (u, v, w) values, and need the centered (x, y, z) ones
125 // around the sphere center's (uc, vc, wc) as: 185 // around the sphere center's (uc, vc, wc) as:
126 // uc = Su / N; The mean value 186 // uc = Su / N; The mean value
127 // x = u - uc; The differnce to the mean. 187 // x = u - uc; The difference to the mean.
128 // 188 //
129 // So: 189 // So:
130 // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2 190 // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2
131 // 191 //
132 // We need the Sxx sum of 2nd orders: 192 // We need the Sxx sum of 2nd orders:
156 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm 216 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
157 // 217 //
158 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy 218 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
159 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N 219 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
160 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N 220 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
221
161 Saa = Suu + Svv + Sww; 222 Saa = Suu + Svv + Sww;
162 yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw); 223
163 yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw); 224 // compute yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw);
164 yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww); 225 //
226 uh = uc; S0 = Saau; S1 = Suu; S2 = Suv; S3 = Suw; compass_solve_helper(); yu = yh;
227
228 // compute yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw);
229 //
230 uh = vc; S0 = Saav; S1 = Suv; S2 = Svv; S3 = Svw; compass_solve_helper(); yv = yh;
231
232 // compute yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww);
233 //
234 uh = wc; S0 = Saaw; S1 = Suw; S2 = Svw; S3 = Sww; compass_solve_helper(); yw = yh;
235
165 236
166 //---- Solve the system -------------------------------------------------- 237 //---- Solve the system --------------------------------------------------
167 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 238 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
168 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 239 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
169 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 240 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
170 // Note this is symmetric, with a positive diagonal, hence 241 //
171 // discriminant is always not null. 242 // Note this is symmetric, with a positive diagonal, hence discriminant is always not null
172 delta = 0.5f / compass_discriminent(0); 243
173 244 //compute delta value
174 // so computed new center, with offset values: 245 calc_discriminant(0); delta = 0.5f / discriminant;
175 uc += compass_discriminent(1) * delta; 246
176 vc += compass_discriminent(2) * delta; 247 // compute new center, with offset values
177 wc += compass_discriminent(3) * delta; 248 calc_discriminant(1); uc += discriminant;
178 249 calc_discriminant(2); vc += discriminant;
179 // add correction due to already applied calibration: 250 calc_discriminant(3); wc += discriminant;
251
252 // add correction due to already applied calibration
180 compass_CX_f += (short)(32768 * uc); 253 compass_CX_f += (short)(32768 * uc);
181 compass_CY_f += (short)(32768 * vc); 254 compass_CY_f += (short)(32768 * vc);
182 compass_CZ_f += (short)(32768 * wc); 255 compass_CZ_f += (short)(32768 * wc);
183 } 256 }
257
258 #endif // _compass