Mercurial > public > hwos_code
annotate src/compass_calib.c @ 305:6d003c08bacd
rename tMODwarning
author | heinrichsweikamp |
---|---|
date | Tue, 02 Jun 2015 16:34:02 +0200 (2015-06-02) |
parents | 7d9edd3b8c86 |
children | ca4556fb60b9 |
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 | |
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 | |
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 |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
29 static float Su, Sv, Sw; // First order moments. |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
30 static float Suu, Svv, Sww, Suv, Suw, Svw; // Second order moments. |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
31 static float Saa; // Suu + Svv + Sww |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
32 static float Saau; // Suuu + Svvu + Swwu // Third order moment. |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
33 static float Saav; // Suuv + Svvv + Swwv |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
34 static float Saaw; // Suuw + Svvw + Swww |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
35 static float yu, yv, yw; // temp solution vector. |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
36 static float uc, vc, wc; // temp sphere's center. |
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 { | |
42 RESET_C_STACK; | |
43 | |
44 compass_N = 0; | |
45 Su = Sv = Sw = 0.0; | |
46 Suu = Svv = Sww = Suv = Suw = Svw = 0.0; | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
47 Saau = Saav = Saaw = 0.0; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
48 compass_CX_f = compass_CY_f = compass_CZ_f = 0; |
0 | 49 } |
50 | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
51 ////////////////////////////////////////////////////////////////////////////// |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
52 |
0 | 53 void compass_add_calibration() |
54 { | |
55 RESET_C_STACK; | |
56 | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
57 // Get filtered/calibrated magnetic direction: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
58 yu = (compass_DX_f - compass_CX_f) / 32768.0f; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
59 yv = (compass_DY_f - compass_CY_f) / 32768.0f; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
60 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f; |
0 | 61 |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
62 // Add to all moments: |
0 | 63 compass_N++; |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
64 |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
65 Su += yu; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
66 Sv += yv; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
67 Sw += yw; |
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 Suu += yu*yu; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
70 Suv += yu*yv; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
71 Suw += yu*yw; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
72 Svv += yv*yv; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
73 Svw += yv*yw; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
74 Sww += yw*yw; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
75 |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
76 Saa = yu*yu + yv*yv + yw*yw; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
77 Saau += yu * Saa; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
78 Saav += yv * Saa; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
79 Saaw += yw * Saa; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
80 } |
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 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
|
85 { |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
86 // Basic symetric matrix: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
87 OVERLAY float a = Suu, d = Suv, g = Suw; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
88 OVERLAY float b = Suv, e = Svv, h = Svw; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
89 OVERLAY float c = Suw, f = Svw, i = Sww; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
90 |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
91 // Substitute a column, if asked to: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
92 if( column==1 ) { a = yu; b = yv; c = yw; } |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
93 if( column==2 ) { d = yu; e = yv; f = yw; } |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
94 if( column==3 ) { g = yu; h = yv; i = yw; } |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
95 |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
96 // Do the math: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
97 return a * (e * i - f * h) |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
98 - b * (d * i - f * g) |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
99 + c * (d * h - e * g); |
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 ////////////////////////////////////////////////////////////////////////////// |
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 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
|
105 { |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
106 return u*uc + v*vc + w*wc; |
0 | 107 } |
108 | |
109 ////////////////////////////////////////////////////////////////////////////// | |
110 | |
111 void compass_solve_calibration() | |
112 { | |
113 OVERLAY float delta; | |
114 RESET_C_STACK; | |
115 | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
116 //---- Compute center of measured magnetic directions -------------------- |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
117 uc = Su/compass_N; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
118 vc = Sv/compass_N; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
119 wc = Sw/compass_N; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
120 |
0 | 121 //---- Normalize partial sums -------------------------------------------- |
122 // | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
123 // We measured the (u, v, w) values, and need the centered (x, y, z) ones |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
124 // around the sphere center's (uc, vc, wc) as: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
125 // uc = Su / N; The mean value |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
126 // x = u - uc; The differnce to the mean. |
0 | 127 // |
128 // So: | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
129 // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2 |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
130 // |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
131 // We need the Sxx sum of 2nd orders: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
132 // Sxx = Suu - 2 uc Su + N*uc*(Su/N) = Suu - uc Su |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
133 Suu -= Su*uc; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
134 Svv -= Sv*vc; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
135 Sww -= Sw*wc; |
0 | 136 |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
137 // (u - uc)(v - vc) = uv - u vc - v uc + uc vc |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
138 // Sxy = Suv - Su vc - Sv uc + N uc vc |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
139 // = Suv - Su vc - N vc uc + N uc vc |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
140 // = Suv - Su vc |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
141 Suv -= Su*vc; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
142 Suw -= Su*wc; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
143 Svw -= Sv*wc; |
0 | 144 |
145 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3 | |
146 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3 | |
147 // Su = 0, um = Sx/N: | |
148 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3 | |
149 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2 | |
150 | |
151 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm) | |
152 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm) | |
153 // | |
154 // Su = 0, Sv = 0, vm = Sy/N: | |
155 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm | |
156 // | |
157 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy | |
158 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N | |
159 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
160 Saa = Suu + Svv + Sww; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
161 yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw); |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
162 yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw); |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
163 yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww); |
0 | 164 |
165 //---- Solve the system -------------------------------------------------- | |
166 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 | |
167 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 | |
168 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 | |
169 // Note this is symetric, with a positiv diagonal, hence | |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
170 // discriminent is always not null. |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
171 delta = 0.5f / compass_discriminent(0); |
0 | 172 |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
173 // So computed new center, with offsetted values: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
174 uc += compass_discriminent(1) * delta; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
175 vc += compass_discriminent(2) * delta; |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
176 wc += compass_discriminent(3) * delta; |
0 | 177 |
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
178 // Add correction due to already applyed calibration: |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
179 compass_CX_f += (short)(32768 * uc); |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
180 compass_CY_f += (short)(32768 * vc); |
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
96
diff
changeset
|
181 compass_CZ_f += (short)(32768 * wc); |
0 | 182 } |