Mercurial > public > hwos_code
annotate src/compass_calib.c @ 634:4050675965ea
3.10 stable release
| author | heinrichsweikamp |
|---|---|
| date | Tue, 28 Apr 2020 17:34:31 +0200 |
| parents | cd58f7fc86db |
| children | bc214815deb2 |
| rev | line source |
|---|---|
| 623 | 1 ///////////////////////////////////////////////////////////////////////////// |
| 2 // | |
| 634 | 3 // compass_calib.c next generation V3.09.4 |
| 623 | 4 // |
| 5 // Calibrate hard-iron for magnetic compass measurements. | |
| 6 // Copyright (c) 2012-2019, JD Gascuel, HeinrichsWeikamp, all rights reserved. | |
| 7 // | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
8 ////////////////////////////////////////////////////////////////////////////// |
| 623 | 9 |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
10 // 2015-05-22 [jDG] Make a smaller calibration code (15.6 --> 6.7 KB). |
| 628 | 11 // 2019-05-14 [rl] make smaller again |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
12 |
| 623 | 13 #include "configuration.inc" |
| 0 | 14 #include "compass.h" |
| 15 | |
| 623 | 16 |
| 17 #ifdef _compass | |
| 18 | |
| 19 | |
| 0 | 20 ////////////////////////////////////////////////////////////////////////////// |
| 623 | 21 // |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
22 // mH: Put compass data into bank 8 (stack) and bank 9 (variables) |
| 623 | 23 // rl: could also be overlaid with p2_deco.c stack... |
| 24 // | |
| 0 | 25 #ifndef UNIX |
| 623 | 26 # pragma udata overlay bank8=0x800 |
| 604 | 27 static char C_STACK[256]; // overlay C-code data stack here |
| 28 # define RESET_C_STACK \ | |
| 29 _asm \ | |
| 30 LFSR 1, 0x800 \ | |
| 31 LFSR 2, 0x800 \ | |
| 32 _endasm | |
| 623 | 33 # pragma udata overlay bank9_compass |
| 634 | 34 # pragma code compass_cal |
| 0 | 35 #else |
| 623 | 36 # define RESET_C_STACK |
| 0 | 37 #endif |
| 38 | |
| 39 ////////////////////////////////////////////////////////////////////////////// | |
| 40 | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
41 static unsigned short int compass_N; |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
42 |
| 628 | 43 static float Su, Sv, Sw; // first order moments |
| 623 | 44 static float Suu, Svv, Sww, Suv, Suw, Svw; // second order moments |
| 604 | 45 static float Saa; // Suu + Svv + Sww |
| 623 | 46 static float Saau; // Suuu + Svvu + Swwu third order moment |
| 47 static float Saav; // Suuv + Svvv + Swwv third order moment | |
| 48 static float Saaw; // Suuw + Svvw + Swww third order moment | |
| 49 | |
| 50 static float yu, yv, yw; // temp solution vector | |
| 51 static float uc, vc, wc; // temp sphere's center | |
| 52 | |
| 628 | 53 static float yh,uh,S0,S1,S2,S3; // transfer vars for compass_solve_helper() |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
54 |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
55 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
56 |
| 628 | 57 void compass_reset_calibration() |
| 0 | 58 { |
| 604 | 59 RESET_C_STACK; |
| 0 | 60 |
| 604 | 61 compass_N = 0; |
| 62 Su = Sv = Sw = 0.0; | |
| 63 Suu = Svv = Sww = 0.0; | |
| 64 Suv = Suw = Svw = 0.0; | |
| 65 Saau = Saav = Saaw = 0.0; | |
| 628 | 66 compass_CX_f = compass_CY_f = compass_CZ_f = 0; |
| 0 | 67 } |
| 68 | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
69 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
70 |
| 628 | 71 void compass_add_calibration() |
| 0 | 72 { |
| 604 | 73 RESET_C_STACK; |
| 0 | 74 |
| 628 | 75 // get filtered/calibrated magnetic direction |
| 604 | 76 yu = (compass_DX_f - compass_CX_f) / 32768.0f; |
| 77 yv = (compass_DY_f - compass_CY_f) / 32768.0f; | |
| 78 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f; | |
| 0 | 79 |
| 628 | 80 // add to all moments |
| 604 | 81 compass_N++; |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
82 |
| 623 | 83 Su += yu; |
| 84 Sv += yv; | |
| 85 Sw += yw; | |
| 86 | |
| 628 | 87 Suu += yu*yu; |
| 623 | 88 Suv += yu*yv; |
| 89 Suw += yu*yw; | |
| 628 | 90 Svv += yv*yv; |
| 623 | 91 Svw += yv*yw; |
| 628 | 92 Sww += yw*yw; |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
93 |
| 628 | 94 Saa = yu*yu + yv*yv + yw*yw; |
| 623 | 95 |
| 604 | 96 Saau += yu * Saa; |
| 97 Saav += yv * Saa; | |
| 98 Saaw += yw * Saa; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
99 } |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
100 |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
101 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
102 |
| 628 | 103 static float compass_discriminent(PARAMETER char column) |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
104 { |
| 604 | 105 // basic symmetric matrix |
| 628 | 106 OVERLAY float a = Suu, d = Suv, g = Suw; |
| 107 OVERLAY float b = Suv, e = Svv, h = Svw; | |
| 108 OVERLAY float c = Suw, f = Svw, i = Sww; | |
| 109 OVERLAY float result; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
110 |
| 604 | 111 // substitute a column, if asked to |
| 112 if( column==1 ) { a = yu; b = yv; c = yw; } | |
| 113 if( column==2 ) { d = yu; e = yv; f = yw; } | |
| 114 if( column==3 ) { g = yu; h = yv; i = yw; } | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
115 |
| 623 | 116 // do the math in a couple of single terms to reduce |
| 117 // the amount of required ".tmpdata" memory | |
| 628 | 118 result = a * (e * i - f * h); |
| 119 result -= b * (d * i - f * g); | |
| 120 result += c * (d * h - e * g); | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
121 |
| 628 | 122 return result; |
| 0 | 123 } |
| 124 | |
| 125 ////////////////////////////////////////////////////////////////////////////// | |
| 126 | |
| 628 | 127 static void compass_solve_helper(void) |
| 0 | 128 { |
| 623 | 129 // computes: |
| 130 // | |
| 131 // yh = S0 - Saa*uh - compass_dotc(Su*uh + 2*S1, Sv*uh + 2*S2, Sw*uh + 2*S3); | |
| 132 // | |
| 133 // with compass_dotc(u,v,w) = u*uc + v*vc + w*wc the equation becomes: | |
| 134 // | |
| 135 // yh = S0 - Saa*uh - ( (Su*uh + 2*S1)*uc + (Sv*uh + 2*S2)*vc + (Sw*uh + 2*S3)*wc ) | |
| 136 // | |
| 137 // this equation is now split into several single terms | |
| 138 // to reduce the amount of required ".tmpdata" memory: | |
| 139 | |
| 140 yh = S0; | |
| 141 yh -= Saa*uh; | |
| 142 yh -= (Su*uh + 2*S1) * uc; | |
| 143 yh -= (Sv*uh + 2*S2) * vc; | |
| 144 yh -= (Sw*uh + 2*S3) * wc; | |
| 628 | 145 |
| 146 return; | |
| 623 | 147 } |
| 148 | |
| 149 ////////////////////////////////////////////////////////////////////////////// | |
| 150 | |
| 628 | 151 void compass_solve_calibration() |
| 623 | 152 { |
| 628 | 153 OVERLAY float delta; |
| 623 | 154 |
| 604 | 155 RESET_C_STACK; |
| 0 | 156 |
| 628 | 157 //---- Compute center of measured magnetic directions -------------------- |
| 158 uc = Su/compass_N; | |
| 159 vc = Sv/compass_N; | |
| 160 wc = Sw/compass_N; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
161 |
| 628 | 162 //---- Normalize partial sums -------------------------------------------- |
| 604 | 163 // |
| 164 // We measured the (u, v, w) values, and need the centered (x, y, z) ones | |
| 165 // around the sphere center's (uc, vc, wc) as: | |
| 623 | 166 // uc = Su / N; The mean value |
| 167 // x = u - uc; The difference to the mean. | |
| 604 | 168 // |
| 169 // So: | |
| 170 // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2 | |
| 171 // | |
| 172 // We need the Sxx sum of 2nd orders: | |
| 173 // Sxx = Suu - 2 uc Su + N*uc*(Su/N) = Suu - uc Su | |
| 174 Suu -= Su*uc; | |
| 175 Svv -= Sv*vc; | |
| 176 Sww -= Sw*wc; | |
| 0 | 177 |
| 604 | 178 // (u - uc)(v - vc) = uv - u vc - v uc + uc vc |
| 179 // Sxy = Suv - Su vc - Sv uc + N uc vc | |
| 180 // = Suv - Su vc - N vc uc + N uc vc | |
| 181 // = Suv - Su vc | |
| 182 Suv -= Su*vc; | |
| 183 Suw -= Su*wc; | |
| 184 Svw -= Sv*wc; | |
| 0 | 185 |
| 604 | 186 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3 |
| 187 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3 | |
| 188 // Su = 0, um = Sx/N: | |
| 189 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3 | |
| 190 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2 | |
| 0 | 191 |
| 604 | 192 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm) |
| 193 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm) | |
| 194 // | |
| 195 // Su = 0, Sv = 0, vm = Sy/N: | |
| 196 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm | |
| 197 // | |
| 198 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy | |
| 199 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N | |
| 200 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N | |
| 623 | 201 |
| 604 | 202 Saa = Suu + Svv + Sww; |
| 623 | 203 |
| 204 // compute yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw); | |
| 628 | 205 uh = uc; S0 = Saau; S1 = Suu; S2 = Suv; S3 = Suw; |
| 206 compass_solve_helper(); | |
| 207 yu = yh; | |
| 623 | 208 |
| 209 // compute yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw); | |
| 628 | 210 uh = vc; S0 = Saav; S1 = Suv; S2 = Svv; S3 = Svw; |
| 211 compass_solve_helper(); | |
| 212 yv = yh; | |
| 623 | 213 |
| 214 // compute yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww); | |
| 628 | 215 uh = wc; S0 = Saaw; S1 = Suw; S2 = Svw; S3 = Sww; |
| 216 compass_solve_helper(); | |
| 217 yw = yh; | |
| 623 | 218 |
| 0 | 219 |
| 604 | 220 //---- Solve the system -------------------------------------------------- |
| 221 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 | |
| 222 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 | |
| 223 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 | |
| 623 | 224 // |
| 225 // Note this is symmetric, with a positive diagonal, hence discriminant is always not null | |
| 226 | |
| 628 | 227 delta = 0.5f / compass_discriminent(0); |
| 0 | 228 |
| 628 | 229 // computed new center, with offset values: |
| 230 uc += compass_discriminent(1) * delta; | |
| 231 vc += compass_discriminent(2) * delta; | |
| 232 wc += compass_discriminent(3) * delta; | |
| 0 | 233 |
| 628 | 234 // add correction due to already applied calibration: |
| 604 | 235 compass_CX_f += (short)(32768 * uc); |
| 236 compass_CY_f += (short)(32768 * vc); | |
| 237 compass_CZ_f += (short)(32768 * wc); | |
| 0 | 238 } |
| 623 | 239 |
| 240 #endif // _compass |