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