Mercurial > public > hwos_code

annotate src/compass_calib.c @ 386:3c55627d5f8f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
some cleanups, jump to 1.90 stable
author heinrichsweikamp
date Wed, 07 Oct 2015 14:46:39 +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 }