Mercurial > public > hwos_code

annotate src/compass_calib.c @ 385:b075fa71b659

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
BUGFIX: +5min for stopwatch during simulation did not work
author heinrichsweikamp
date Wed, 07 Oct 2015 13:14:09 +0200
parents 7d9edd3b8c86
children ca4556fb60b9
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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
heinrichsweikamp
parents:
diff changeset
8 #include "compass.h"
heinrichsweikamp
parents:
diff changeset
9
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
12 #ifndef UNIX
heinrichsweikamp
parents:
diff changeset
13 # pragma udata overlay bank8=0x800
heinrichsweikamp
parents:
diff changeset
14 static char C_STACK[256]; // Overlay C-code data stack here.
heinrichsweikamp
parents:
diff changeset
15 # define RESET_C_STACK \
heinrichsweikamp
parents:
diff changeset
16 _asm \
heinrichsweikamp
parents:
diff changeset
17 LFSR 1, 0x800 \
heinrichsweikamp
parents:
diff changeset
18 LFSR 2, 0x800 \
heinrichsweikamp
parents:
diff changeset
19 _endasm
heinrichsweikamp
parents:
diff changeset
20 # pragma udata overlay bank9_compass
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
23 #endif
heinrichsweikamp
parents:
diff changeset
24
heinrichsweikamp
parents:
diff changeset
25 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
40 void compass_reset_calibration()
heinrichsweikamp
parents:
diff changeset
41 {
heinrichsweikamp
parents:
diff changeset
42 RESET_C_STACK;
heinrichsweikamp
parents:
diff changeset
43
heinrichsweikamp
parents:
diff changeset
44 compass_N = 0;
heinrichsweikamp
parents:
diff changeset
45 Su = Sv = Sw = 0.0;
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
49 }
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
53 void compass_add_calibration()
heinrichsweikamp
parents:
diff changeset
54 {
heinrichsweikamp
parents:
diff changeset
55 RESET_C_STACK;
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
107 }
heinrichsweikamp
parents:
diff changeset
108
heinrichsweikamp
parents:
diff changeset
109 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
110
heinrichsweikamp
parents:
diff changeset
111 void compass_solve_calibration()
heinrichsweikamp
parents:
diff changeset
112 {
heinrichsweikamp
parents:
diff changeset
113 OVERLAY float delta;
heinrichsweikamp
parents:
diff changeset
114 RESET_C_STACK;
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
121 //---- Normalize partial sums --------------------------------------------
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
127 //
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
144
heinrichsweikamp
parents:
diff changeset
145 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
heinrichsweikamp
parents:
diff changeset
146 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
heinrichsweikamp
parents:
diff changeset
147 // Su = 0, um = Sx/N:
heinrichsweikamp
parents:
diff changeset
148 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
heinrichsweikamp
parents:
diff changeset
149 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
heinrichsweikamp
parents:
diff changeset
150
heinrichsweikamp
parents:
diff changeset
151 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
heinrichsweikamp
parents:
diff changeset
152 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
heinrichsweikamp
parents:
diff changeset
153 //
heinrichsweikamp
parents:
diff changeset
154 // Su = 0, Sv = 0, vm = Sy/N:
heinrichsweikamp
parents:
diff changeset
155 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
heinrichsweikamp
parents:
diff changeset
156 //
heinrichsweikamp
parents:
diff changeset
157 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
heinrichsweikamp
parents:
diff changeset
158 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
164
heinrichsweikamp
parents:
diff changeset
165 //---- Solve the system --------------------------------------------------
heinrichsweikamp
parents:
diff changeset
166 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
heinrichsweikamp
parents:
diff changeset
167 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
heinrichsweikamp
parents:
diff changeset
168 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
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
heinrichsweikamp
parents:
diff changeset
182 }