Mercurial > public > hwos_code
annotate src/compass_calib.c @ 623:c40025d8e750
3.03 beta released
| author | heinrichsweikamp |
|---|---|
| date | Mon, 03 Jun 2019 14:01:48 +0200 |
| parents | ca4556fb60b9 |
| children | cd58f7fc86db |
| rev | line source |
|---|---|
| 623 | 1 ///////////////////////////////////////////////////////////////////////////// |
| 2 // | |
| 3 // compass_calib.c next generation V3.03.4 | |
| 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). |
| 623 | 11 // 2019-05-14 [rl] make it even smaller again by another 2.000 byte |
|
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 |
| 0 | 34 #else |
| 623 | 35 # define RESET_C_STACK |
| 0 | 36 #endif |
| 37 | |
| 38 ////////////////////////////////////////////////////////////////////////////// | |
| 39 | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
40 static unsigned short int compass_N; |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
41 |
| 623 | 42 static float Su, Sv, Sw; // first order moments |
| 43 static float Suu, Svv, Sww, Suv, Suw, Svw; // second order moments | |
| 604 | 44 static float Saa; // Suu + Svv + Sww |
| 623 | 45 static float Saau; // Suuu + Svvu + Swwu third order moment |
| 46 static float Saav; // Suuv + Svvv + Swwv third order moment | |
| 47 static float Saaw; // Suuw + Svvw + Swww third order moment | |
| 48 | |
| 49 static float yu, yv, yw; // temp solution vector | |
| 50 static float uc, vc, wc; // temp sphere's center | |
| 51 | |
| 52 static float yh, uh, S0, S1, S2, S3; // transfer vars for compass_solve_helper() | |
| 53 static float discriminant, delta; // transfer vars for calc_discriminant() | |
| 54 | |
|
282
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 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
57 |
| 623 | 58 #ifndef UNIX |
| 59 # pragma code compass_calib | |
| 60 #endif | |
| 61 | |
| 62 ////////////////////////////////////////////////////////////////////////////// | |
| 63 | |
| 64 void compass_reset_calibration() // 202 byte | |
| 0 | 65 { |
| 604 | 66 RESET_C_STACK; |
| 0 | 67 |
| 604 | 68 compass_N = 0; |
| 69 Su = Sv = Sw = 0.0; | |
| 70 Suu = Svv = Sww = 0.0; | |
| 71 Suv = Suw = Svw = 0.0; | |
| 72 Saau = Saav = Saaw = 0.0; | |
| 623 | 73 compass_CX_f = compass_CY_f = compass_CZ_f = 0; // int16 |
| 0 | 74 } |
| 75 | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
76 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
77 |
| 623 | 78 void compass_add_calibration() // 1488 byte |
| 0 | 79 { |
| 623 | 80 overlay float SQR_yu, SQR_yv, SQR_yw; // squared values; |
| 81 | |
| 604 | 82 RESET_C_STACK; |
| 0 | 83 |
| 623 | 84 // get filtered/calibrated magnetic direction // 276 byte |
| 604 | 85 yu = (compass_DX_f - compass_CX_f) / 32768.0f; |
| 86 yv = (compass_DY_f - compass_CY_f) / 32768.0f; | |
| 87 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f; | |
| 0 | 88 |
| 623 | 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 | |
| 604 | 95 compass_N++; |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
96 |
| 623 | 97 // add to all moments // 156 byte |
| 98 Su += yu; | |
| 99 Sv += yv; | |
| 100 Sw += yw; | |
| 101 | |
| 102 // // 156 byte | |
| 103 Suu += SQR_yu; | |
| 104 Svv += SQR_yv; | |
| 105 Sww += SQR_yw; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
106 |
| 623 | 107 // // 312 byte |
| 108 Suv += yu*yv; | |
| 109 Suw += yu*yw; | |
| 110 Svw += yv*yw; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
111 |
| 623 | 112 // // 104 byte |
| 113 Saa = SQR_yu + SQR_yv + SQR_yw; | |
| 114 | |
| 115 | |
| 116 // // 312 byte | |
| 604 | 117 Saau += yu * Saa; |
| 118 Saav += yv * Saa; | |
| 119 Saaw += yw * Saa; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
120 } |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
121 |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
122 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
123 |
| 623 | 124 static void calc_discriminant(PARAMETER char column) |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
125 { |
| 604 | 126 // basic symmetric matrix |
| 623 | 127 overlay float a = Suu, d = Suv, g = Suw; |
| 128 overlay float b = Suv, e = Svv, h = Svw; | |
| 129 overlay float c = Suw, f = Svw, i = Sww; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
130 |
| 604 | 131 // substitute a column, if asked to |
| 132 if( column==1 ) { a = yu; b = yv; c = yw; } | |
| 133 if( column==2 ) { d = yu; e = yv; f = yw; } | |
| 134 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
|
135 |
| 623 | 136 // do the math in a couple of single terms to reduce |
| 137 // the amount of required ".tmpdata" memory | |
| 138 discriminant = a * (e * i - f * h); | |
| 139 discriminant -= b * (d * i - f * g); | |
| 140 discriminant += c * (d * h - e * g); | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
141 |
| 623 | 142 // apply delta if column = 1/2/3 |
| 143 if( column ) discriminant *= delta; | |
| 0 | 144 } |
| 145 | |
| 146 ////////////////////////////////////////////////////////////////////////////// | |
| 147 | |
| 623 | 148 static void compass_solve_helper(void) // 338 byte |
| 0 | 149 { |
| 623 | 150 // computes: |
| 151 // | |
| 152 // yh = S0 - Saa*uh - compass_dotc(Su*uh + 2*S1, Sv*uh + 2*S2, Sw*uh + 2*S3); | |
| 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 | |
| 604 | 174 RESET_C_STACK; |
| 0 | 175 |
| 623 | 176 // compute center of measured magnetic directions // 200 byte |
| 177 inv_compass_N = 1 / compass_N; | |
| 178 uc = Su * inv_compass_N; | |
| 179 vc = Sv * inv_compass_N; | |
| 180 wc = Sw * inv_compass_N; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
181 |
| 623 | 182 // normalize partial sums |
| 604 | 183 // |
| 184 // We measured the (u, v, w) values, and need the centered (x, y, z) ones | |
| 185 // around the sphere center's (uc, vc, wc) as: | |
| 623 | 186 // uc = Su / N; The mean value |
| 187 // x = u - uc; The difference to the mean. | |
| 604 | 188 // |
| 189 // So: | |
| 190 // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2 | |
| 191 // | |
| 192 // We need the Sxx sum of 2nd orders: | |
| 193 // Sxx = Suu - 2 uc Su + N*uc*(Su/N) = Suu - uc Su | |
| 194 Suu -= Su*uc; | |
| 195 Svv -= Sv*vc; | |
| 196 Sww -= Sw*wc; | |
| 0 | 197 |
| 604 | 198 // (u - uc)(v - vc) = uv - u vc - v uc + uc vc |
| 199 // Sxy = Suv - Su vc - Sv uc + N uc vc | |
| 200 // = Suv - Su vc - N vc uc + N uc vc | |
| 201 // = Suv - Su vc | |
| 202 Suv -= Su*vc; | |
| 203 Suw -= Su*wc; | |
| 204 Svw -= Sv*wc; | |
| 0 | 205 |
| 604 | 206 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3 |
| 207 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3 | |
| 208 // Su = 0, um = Sx/N: | |
| 209 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3 | |
| 210 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2 | |
| 0 | 211 |
| 604 | 212 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm) |
| 213 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm) | |
| 214 // | |
| 215 // Su = 0, Sv = 0, vm = Sy/N: | |
| 216 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm | |
| 217 // | |
| 218 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy | |
| 219 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N | |
| 220 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N | |
| 623 | 221 |
| 604 | 222 Saa = Suu + Svv + Sww; |
| 623 | 223 |
| 224 // compute yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw); | |
| 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 | |
| 0 | 236 |
| 604 | 237 //---- Solve the system -------------------------------------------------- |
| 238 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 | |
| 239 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 | |
| 240 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 | |
| 623 | 241 // |
| 242 // Note this is symmetric, with a positive diagonal, hence discriminant is always not null | |
| 243 | |
| 244 //compute delta value | |
| 245 calc_discriminant(0); delta = 0.5f / discriminant; | |
| 0 | 246 |
| 623 | 247 // compute new center, with offset values |
| 248 calc_discriminant(1); uc += discriminant; | |
| 249 calc_discriminant(2); vc += discriminant; | |
| 250 calc_discriminant(3); wc += discriminant; | |
| 0 | 251 |
| 623 | 252 // add correction due to already applied calibration |
| 604 | 253 compass_CX_f += (short)(32768 * uc); |
| 254 compass_CY_f += (short)(32768 * vc); | |
| 255 compass_CZ_f += (short)(32768 * wc); | |
| 0 | 256 } |
| 623 | 257 |
| 258 #endif // _compass |