Mercurial > public > hwos_code
annotate src/compass_calib.c @ 628:cd58f7fc86db
3.05 stable work
| author | heinrichsweikamp |
|---|---|
| date | Thu, 19 Sep 2019 12:01:29 +0200 |
| parents | c40025d8e750 |
| children | 4050675965ea |
| 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). |
| 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 |
| 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 |
| 628 | 42 static float Su, Sv, Sw; // first order moments |
| 623 | 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 | |
| 628 | 52 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
|
53 |
|
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 |
| 628 | 56 void compass_reset_calibration() |
| 0 | 57 { |
| 604 | 58 RESET_C_STACK; |
| 0 | 59 |
| 604 | 60 compass_N = 0; |
| 61 Su = Sv = Sw = 0.0; | |
| 62 Suu = Svv = Sww = 0.0; | |
| 63 Suv = Suw = Svw = 0.0; | |
| 64 Saau = Saav = Saaw = 0.0; | |
| 628 | 65 compass_CX_f = compass_CY_f = compass_CZ_f = 0; |
| 0 | 66 } |
| 67 | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
68 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
69 |
| 628 | 70 void compass_add_calibration() |
| 0 | 71 { |
| 604 | 72 RESET_C_STACK; |
| 0 | 73 |
| 628 | 74 // get filtered/calibrated magnetic direction |
| 604 | 75 yu = (compass_DX_f - compass_CX_f) / 32768.0f; |
| 76 yv = (compass_DY_f - compass_CY_f) / 32768.0f; | |
| 77 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f; | |
| 0 | 78 |
| 628 | 79 // add to all moments |
| 604 | 80 compass_N++; |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
81 |
| 623 | 82 Su += yu; |
| 83 Sv += yv; | |
| 84 Sw += yw; | |
| 85 | |
| 628 | 86 Suu += yu*yu; |
| 623 | 87 Suv += yu*yv; |
| 88 Suw += yu*yw; | |
| 628 | 89 Svv += yv*yv; |
| 623 | 90 Svw += yv*yw; |
| 628 | 91 Sww += yw*yw; |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
92 |
| 628 | 93 Saa = yu*yu + yv*yv + yw*yw; |
| 623 | 94 |
| 604 | 95 Saau += yu * Saa; |
| 96 Saav += yv * Saa; | |
| 97 Saaw += yw * Saa; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
98 } |
|
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 |
| 628 | 102 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
|
103 { |
| 604 | 104 // basic symmetric matrix |
| 628 | 105 OVERLAY float a = Suu, d = Suv, g = Suw; |
| 106 OVERLAY float b = Suv, e = Svv, h = Svw; | |
| 107 OVERLAY float c = Suw, f = Svw, i = Sww; | |
| 108 OVERLAY float result; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
109 |
| 604 | 110 // substitute a column, if asked to |
| 111 if( column==1 ) { a = yu; b = yv; c = yw; } | |
| 112 if( column==2 ) { d = yu; e = yv; f = yw; } | |
| 113 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
|
114 |
| 623 | 115 // do the math in a couple of single terms to reduce |
| 116 // the amount of required ".tmpdata" memory | |
| 628 | 117 result = a * (e * i - f * h); |
| 118 result -= b * (d * i - f * g); | |
| 119 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
|
120 |
| 628 | 121 return result; |
| 0 | 122 } |
| 123 | |
| 124 ////////////////////////////////////////////////////////////////////////////// | |
| 125 | |
| 628 | 126 static void compass_solve_helper(void) |
| 0 | 127 { |
| 623 | 128 // computes: |
| 129 // | |
| 130 // yh = S0 - Saa*uh - compass_dotc(Su*uh + 2*S1, Sv*uh + 2*S2, Sw*uh + 2*S3); | |
| 131 // | |
| 132 // with compass_dotc(u,v,w) = u*uc + v*vc + w*wc the equation becomes: | |
| 133 // | |
| 134 // yh = S0 - Saa*uh - ( (Su*uh + 2*S1)*uc + (Sv*uh + 2*S2)*vc + (Sw*uh + 2*S3)*wc ) | |
| 135 // | |
| 136 // this equation is now split into several single terms | |
| 137 // to reduce the amount of required ".tmpdata" memory: | |
| 138 | |
| 139 yh = S0; | |
| 140 yh -= Saa*uh; | |
| 141 yh -= (Su*uh + 2*S1) * uc; | |
| 142 yh -= (Sv*uh + 2*S2) * vc; | |
| 143 yh -= (Sw*uh + 2*S3) * wc; | |
| 628 | 144 |
| 145 return; | |
| 623 | 146 } |
| 147 | |
| 148 ////////////////////////////////////////////////////////////////////////////// | |
| 149 | |
| 628 | 150 void compass_solve_calibration() |
| 623 | 151 { |
| 628 | 152 OVERLAY float delta; |
| 623 | 153 |
| 604 | 154 RESET_C_STACK; |
| 0 | 155 |
| 628 | 156 //---- Compute center of measured magnetic directions -------------------- |
| 157 uc = Su/compass_N; | |
| 158 vc = Sv/compass_N; | |
| 159 wc = Sw/compass_N; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
160 |
| 628 | 161 //---- Normalize partial sums -------------------------------------------- |
| 604 | 162 // |
| 163 // We measured the (u, v, w) values, and need the centered (x, y, z) ones | |
| 164 // around the sphere center's (uc, vc, wc) as: | |
| 623 | 165 // uc = Su / N; The mean value |
| 166 // x = u - uc; The difference to the mean. | |
| 604 | 167 // |
| 168 // So: | |
| 169 // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2 | |
| 170 // | |
| 171 // We need the Sxx sum of 2nd orders: | |
| 172 // Sxx = Suu - 2 uc Su + N*uc*(Su/N) = Suu - uc Su | |
| 173 Suu -= Su*uc; | |
| 174 Svv -= Sv*vc; | |
| 175 Sww -= Sw*wc; | |
| 0 | 176 |
| 604 | 177 // (u - uc)(v - vc) = uv - u vc - v uc + uc vc |
| 178 // Sxy = Suv - Su vc - Sv uc + N uc vc | |
| 179 // = Suv - Su vc - N vc uc + N uc vc | |
| 180 // = Suv - Su vc | |
| 181 Suv -= Su*vc; | |
| 182 Suw -= Su*wc; | |
| 183 Svw -= Sv*wc; | |
| 0 | 184 |
| 604 | 185 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3 |
| 186 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3 | |
| 187 // Su = 0, um = Sx/N: | |
| 188 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3 | |
| 189 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2 | |
| 0 | 190 |
| 604 | 191 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm) |
| 192 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm) | |
| 193 // | |
| 194 // Su = 0, Sv = 0, vm = Sy/N: | |
| 195 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm | |
| 196 // | |
| 197 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy | |
| 198 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N | |
| 199 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N | |
| 623 | 200 |
| 604 | 201 Saa = Suu + Svv + Sww; |
| 623 | 202 |
| 203 // compute yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw); | |
| 628 | 204 uh = uc; S0 = Saau; S1 = Suu; S2 = Suv; S3 = Suw; |
| 205 compass_solve_helper(); | |
| 206 yu = yh; | |
| 623 | 207 |
| 208 // compute yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw); | |
| 628 | 209 uh = vc; S0 = Saav; S1 = Suv; S2 = Svv; S3 = Svw; |
| 210 compass_solve_helper(); | |
| 211 yv = yh; | |
| 623 | 212 |
| 213 // compute yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww); | |
| 628 | 214 uh = wc; S0 = Saaw; S1 = Suw; S2 = Svw; S3 = Sww; |
| 215 compass_solve_helper(); | |
| 216 yw = yh; | |
| 623 | 217 |
| 0 | 218 |
| 604 | 219 //---- Solve the system -------------------------------------------------- |
| 220 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 | |
| 221 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 | |
| 222 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 | |
| 623 | 223 // |
| 224 // Note this is symmetric, with a positive diagonal, hence discriminant is always not null | |
| 225 | |
| 628 | 226 delta = 0.5f / compass_discriminent(0); |
| 0 | 227 |
| 628 | 228 // computed new center, with offset values: |
| 229 uc += compass_discriminent(1) * delta; | |
| 230 vc += compass_discriminent(2) * delta; | |
| 231 wc += compass_discriminent(3) * delta; | |
| 0 | 232 |
| 628 | 233 // add correction due to already applied calibration: |
| 604 | 234 compass_CX_f += (short)(32768 * uc); |
| 235 compass_CY_f += (short)(32768 * vc); | |
| 236 compass_CZ_f += (short)(32768 * wc); | |
| 0 | 237 } |
| 623 | 238 |
| 239 #endif // _compass |