Mercurial > public > hwos_code
annotate src/compass_calib.c @ 605:5ce603c29750
gas color coding fixed
| author | heinrichsweikamp |
|---|---|
| date | Sun, 25 Nov 2018 13:21:52 +0100 |
| parents | ca4556fb60b9 |
| children | c40025d8e750 |
| rev | line source |
|---|---|
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
1 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
2 /// compass_calib.c |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
3 /// Calibrate hard-iron for magnetic compass measurements. |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
4 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved. |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
5 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
6 // 2015-05-22 [jDG] Make a smaller calibration code (15.6 --> 6.7 KB). |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
7 |
| 0 | 8 #include "compass.h" |
| 9 | |
| 10 ////////////////////////////////////////////////////////////////////////////// | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
11 // mH: Put compass data into bank 8 (stack) and bank 9 (variables) |
| 0 | 12 #ifndef UNIX |
| 13 # pragma udata overlay bank8=0x800 | |
| 604 | 14 static char C_STACK[256]; // overlay C-code data stack here |
| 15 # define RESET_C_STACK \ | |
| 16 _asm \ | |
| 17 LFSR 1, 0x800 \ | |
| 18 LFSR 2, 0x800 \ | |
| 19 _endasm | |
| 0 | 20 # pragma udata overlay bank9_compass |
| 21 #else | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
22 # define RESET_C_STACK |
| 0 | 23 #endif |
| 24 | |
| 25 ////////////////////////////////////////////////////////////////////////////// | |
| 26 | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
27 static unsigned short int compass_N; |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
28 |
| 604 | 29 static float Su, Sv, Sw; // first order moments |
| 30 static float Suu, Svv, Sww, Suv, Suw, Svw; // second order moments | |
| 31 static float Saa; // Suu + Svv + Sww | |
| 32 static float Saau; // Suuu + Svvu + Swwu // third order moment | |
| 33 static float Saav; // Suuv + Svvv + Swwv | |
| 34 static float Saaw; // Suuw + Svvw + Swww | |
| 35 static float yu, yv, yw; // temp solution vector | |
| 36 static float uc, vc, wc; // temp sphere's center | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
37 |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
38 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
39 |
| 0 | 40 void compass_reset_calibration() |
| 41 { | |
| 604 | 42 RESET_C_STACK; |
| 0 | 43 |
| 604 | 44 compass_N = 0; |
| 45 Su = Sv = Sw = 0.0; | |
| 46 Suu = Svv = Sww = 0.0; | |
| 47 Suv = Suw = Svw = 0.0; | |
| 48 Saau = Saav = Saaw = 0.0; | |
| 49 compass_CX_f = compass_CY_f = compass_CZ_f = 0; | |
| 0 | 50 } |
| 51 | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
52 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
53 |
| 0 | 54 void compass_add_calibration() |
| 55 { | |
| 604 | 56 RESET_C_STACK; |
| 0 | 57 |
| 604 | 58 // get filtered/calibrated magnetic direction |
| 59 yu = (compass_DX_f - compass_CX_f) / 32768.0f; | |
| 60 yv = (compass_DY_f - compass_CY_f) / 32768.0f; | |
| 61 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f; | |
| 0 | 62 |
| 604 | 63 // add to all moments |
| 64 compass_N++; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
65 |
| 604 | 66 Su += yu; |
| 67 Sv += yv; | |
| 68 Sw += yw; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
69 |
| 604 | 70 Suu += yu*yu; |
| 71 Suv += yu*yv; | |
| 72 Suw += yu*yw; | |
| 73 Svv += yv*yv; | |
| 74 Svw += yv*yw; | |
| 75 Sww += yw*yw; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
76 |
| 604 | 77 Saa = yu*yu + yv*yv + yw*yw; |
| 78 Saau += yu * Saa; | |
| 79 Saav += yv * Saa; | |
| 80 Saaw += yw * Saa; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
81 } |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
82 |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
83 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
84 |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
85 static float compass_discriminent(PARAMETER char column) |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
86 { |
| 604 | 87 // basic symmetric matrix |
| 88 OVERLAY float a = Suu, d = Suv, g = Suw; | |
| 89 OVERLAY float b = Suv, e = Svv, h = Svw; | |
| 90 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
|
91 |
| 604 | 92 // substitute a column, if asked to |
| 93 if( column==1 ) { a = yu; b = yv; c = yw; } | |
| 94 if( column==2 ) { d = yu; e = yv; f = yw; } | |
| 95 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
|
96 |
| 604 | 97 // do the math |
| 98 return a * (e * i - f * h) | |
| 99 - b * (d * i - f * g) | |
| 100 + c * (d * h - e * g); | |
|
282
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 |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
103 ////////////////////////////////////////////////////////////////////////////// |
|
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 static float compass_dotc(PARAMETER float u, float v, float w) |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
106 { |
| 604 | 107 return u*uc + v*vc + w*wc; |
| 0 | 108 } |
| 109 | |
| 110 ////////////////////////////////////////////////////////////////////////////// | |
| 111 | |
| 112 void compass_solve_calibration() | |
| 113 { | |
| 604 | 114 OVERLAY float delta; |
| 115 RESET_C_STACK; | |
| 0 | 116 |
| 604 | 117 //---- Compute center of measured magnetic directions -------------------- |
| 118 uc = Su/compass_N; | |
| 119 vc = Sv/compass_N; | |
| 120 wc = Sw/compass_N; | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
121 |
| 604 | 122 //---- Normalize partial sums -------------------------------------------- |
| 123 // | |
| 124 // 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: | |
| 126 // uc = Su / N; The mean value | |
| 127 // x = u - uc; The differnce to the mean. | |
| 128 // | |
| 129 // So: | |
| 130 // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2 | |
| 131 // | |
| 132 // We need the Sxx sum of 2nd orders: | |
| 133 // Sxx = Suu - 2 uc Su + N*uc*(Su/N) = Suu - uc Su | |
| 134 Suu -= Su*uc; | |
| 135 Svv -= Sv*vc; | |
| 136 Sww -= Sw*wc; | |
| 0 | 137 |
| 604 | 138 // (u - uc)(v - vc) = uv - u vc - v uc + uc vc |
| 139 // Sxy = Suv - Su vc - Sv uc + N uc vc | |
| 140 // = Suv - Su vc - N vc uc + N uc vc | |
| 141 // = Suv - Su vc | |
| 142 Suv -= Su*vc; | |
| 143 Suw -= Su*wc; | |
| 144 Svw -= Sv*wc; | |
| 0 | 145 |
| 604 | 146 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3 |
| 147 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3 | |
| 148 // Su = 0, um = Sx/N: | |
| 149 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3 | |
| 150 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2 | |
| 0 | 151 |
| 604 | 152 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm) |
| 153 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm) | |
| 154 // | |
| 155 // Su = 0, Sv = 0, vm = Sy/N: | |
| 156 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm | |
| 157 // | |
| 158 // 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 | |
| 160 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N | |
| 161 Saa = Suu + Svv + Sww; | |
| 162 yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw); | |
| 163 yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw); | |
| 164 yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww); | |
| 0 | 165 |
| 604 | 166 //---- Solve the system -------------------------------------------------- |
| 167 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 | |
| 168 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 | |
| 169 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 | |
| 170 // Note this is symmetric, with a positive diagonal, hence | |
| 171 // discriminant is always not null. | |
| 172 delta = 0.5f / compass_discriminent(0); | |
| 0 | 173 |
| 604 | 174 // so computed new center, with offset values: |
| 175 uc += compass_discriminent(1) * delta; | |
| 176 vc += compass_discriminent(2) * delta; | |
| 177 wc += compass_discriminent(3) * delta; | |
| 0 | 178 |
| 604 | 179 // add correction due to already applied calibration: |
| 180 compass_CX_f += (short)(32768 * uc); | |
| 181 compass_CY_f += (short)(32768 * vc); | |
| 182 compass_CZ_f += (short)(32768 * wc); | |
| 0 | 183 } |