annotate src/compass.c @ 467:6ca155fc1509

battery gauge handling
author heinrichsweikamp
date Sun, 20 Nov 2016 12:54:36 +0100 (2016-11-20)
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: 214
diff changeset
1 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents: 214
diff changeset
2 /// compass.c
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents: 214
diff changeset
3 /// Compute north direction from magnetic/acceleration measures.
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents: 214
diff changeset
4 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
0
heinrichsweikamp
parents:
diff changeset
5 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
6 // HISTORY
heinrichsweikamp
parents:
diff changeset
7 // 2012-12-01 [jDG] Creation
heinrichsweikamp
parents:
diff changeset
8 // 2012-12-23 [jDG] Added filtering.
heinrichsweikamp
parents:
diff changeset
9 // 2012-12-30 [jDG] Added calibration (spherical best fit).
282
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents: 214
diff changeset
10 // 2015-05-22 [jDG] Minor cleanups. Smaller calibration code.
0
heinrichsweikamp
parents:
diff changeset
11
heinrichsweikamp
parents:
diff changeset
12 #include "compass.h"
heinrichsweikamp
parents:
diff changeset
13
heinrichsweikamp
parents:
diff changeset
14 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
15 // mH: Crude work-around, needs to be made right
heinrichsweikamp
parents:
diff changeset
16 #ifndef UNIX
heinrichsweikamp
parents:
diff changeset
17 # pragma udata overlay bank8=0x800
heinrichsweikamp
parents:
diff changeset
18 static char C_STACK[256]; // Overlay C-code data stack here.
heinrichsweikamp
parents:
diff changeset
19 # define RESET_C_STACK \
heinrichsweikamp
parents:
diff changeset
20 _asm \
heinrichsweikamp
parents:
diff changeset
21 LFSR 1, 0x800 \
heinrichsweikamp
parents:
diff changeset
22 LFSR 2, 0x800 \
heinrichsweikamp
parents:
diff changeset
23 _endasm
heinrichsweikamp
parents:
diff changeset
24 # pragma udata overlay bank9_compass
282
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents: 214
diff changeset
25 # pragma code compass_run
0
heinrichsweikamp
parents:
diff changeset
26 #else
282
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents: 214
diff changeset
27 # define RESET_C_STACK
0
heinrichsweikamp
parents:
diff changeset
28 #endif
heinrichsweikamp
parents:
diff changeset
29
heinrichsweikamp
parents:
diff changeset
30 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
31 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
heinrichsweikamp
parents:
diff changeset
32 //
heinrichsweikamp
parents:
diff changeset
33 #define K1 (5701) // Needs K1/2**16
heinrichsweikamp
parents:
diff changeset
34 #define K2 (1645) // Needs K2/2**48 WAS NEGATIV
heinrichsweikamp
parents:
diff changeset
35 #define K3 ( 446) // Needs K3/2**80
heinrichsweikamp
parents:
diff changeset
36
heinrichsweikamp
parents:
diff changeset
37 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
38 // Interface to assembleur multiplies
heinrichsweikamp
parents:
diff changeset
39 Int16 umul(PARAMETER Int16 a, PARAMETER Int16 b)
heinrichsweikamp
parents:
diff changeset
40 {
heinrichsweikamp
parents:
diff changeset
41 extern Int16 compass_umul(void);
heinrichsweikamp
parents:
diff changeset
42 extern Int16 compass_a, compass_b;
heinrichsweikamp
parents:
diff changeset
43 compass_a = a;
heinrichsweikamp
parents:
diff changeset
44 compass_b = b;
heinrichsweikamp
parents:
diff changeset
45 return compass_umul();
heinrichsweikamp
parents:
diff changeset
46 }
heinrichsweikamp
parents:
diff changeset
47
heinrichsweikamp
parents:
diff changeset
48 Int16 imul(PARAMETER Int16 a, PARAMETER Int16 b)
heinrichsweikamp
parents:
diff changeset
49 {
heinrichsweikamp
parents:
diff changeset
50 extern Int16 compass_imul(void);
heinrichsweikamp
parents:
diff changeset
51 extern Int16 compass_a, compass_b;
heinrichsweikamp
parents:
diff changeset
52 compass_a = a;
heinrichsweikamp
parents:
diff changeset
53 compass_b = b;
heinrichsweikamp
parents:
diff changeset
54 return compass_imul();
heinrichsweikamp
parents:
diff changeset
55 }
heinrichsweikamp
parents:
diff changeset
56
heinrichsweikamp
parents:
diff changeset
57 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
58 /// Returns a / b * 2**16
heinrichsweikamp
parents:
diff changeset
59 ///
heinrichsweikamp
parents:
diff changeset
60 /// A 16/16 -> 16 bits divide, returning a scalled result.
heinrichsweikamp
parents:
diff changeset
61 /// Used to multiply fractional numbers in the range 0..1,
heinrichsweikamp
parents:
diff changeset
62 /// represented as 0..32767.
heinrichsweikamp
parents:
diff changeset
63 Int16 udiv(PARAMETER Int16 a, PARAMETER Int16 b)
heinrichsweikamp
parents:
diff changeset
64 {
heinrichsweikamp
parents:
diff changeset
65 OVERLAY Int16 d, r;
heinrichsweikamp
parents:
diff changeset
66
heinrichsweikamp
parents:
diff changeset
67 //---- Pre-scale both numerator and denominator --------------------------
heinrichsweikamp
parents:
diff changeset
68 while( (((a>>8) | (b>>8)) & 0xC0) == 0 )
heinrichsweikamp
parents:
diff changeset
69 {
heinrichsweikamp
parents:
diff changeset
70 a <<= 1;
heinrichsweikamp
parents:
diff changeset
71 b <<= 1;
heinrichsweikamp
parents:
diff changeset
72 }
heinrichsweikamp
parents:
diff changeset
73
heinrichsweikamp
parents:
diff changeset
74 //---- Make division trials ----------------------------------------------
heinrichsweikamp
parents:
diff changeset
75 d = 0x4000; // Starts with 0.5, because 1.0 is sign bit.
heinrichsweikamp
parents:
diff changeset
76 b >>= 1; // Hence pre-shift b.
heinrichsweikamp
parents:
diff changeset
77 r = 0;
heinrichsweikamp
parents:
diff changeset
78 do {
heinrichsweikamp
parents:
diff changeset
79 if( a >= b ) { // a is big enough ?
heinrichsweikamp
parents:
diff changeset
80 a -= b; // then count d times b out of it.
heinrichsweikamp
parents:
diff changeset
81 r |= d; // and accumulate that bit.
heinrichsweikamp
parents:
diff changeset
82 }
heinrichsweikamp
parents:
diff changeset
83 b >>= 1; // then loop trying twice smaller.
heinrichsweikamp
parents:
diff changeset
84 d >>= 1;
heinrichsweikamp
parents:
diff changeset
85 } while( b );
heinrichsweikamp
parents:
diff changeset
86 return r;
heinrichsweikamp
parents:
diff changeset
87 }
heinrichsweikamp
parents:
diff changeset
88
heinrichsweikamp
parents:
diff changeset
89 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
90 /// Computes atan(y/x) in Angle, for x, y in range 0..32767
heinrichsweikamp
parents:
diff changeset
91 ///
heinrichsweikamp
parents:
diff changeset
92 /// Results a single quadrant Angle, in the range 0 .. Q_PI/2
heinrichsweikamp
parents:
diff changeset
93 Angle utan(PARAMETER Int16 y, PARAMETER Int16 x)
heinrichsweikamp
parents:
diff changeset
94 {
heinrichsweikamp
parents:
diff changeset
95 OVERLAY Int16 ratio, angle, x2, x3;
heinrichsweikamp
parents:
diff changeset
96
heinrichsweikamp
parents:
diff changeset
97 //---- Handle zero divisor -----------------------------------------------
heinrichsweikamp
parents:
diff changeset
98 if( x == 0 )
heinrichsweikamp
parents:
diff changeset
99 return (y == 0) ? 0 : Q_PIO2;
heinrichsweikamp
parents:
diff changeset
100
heinrichsweikamp
parents:
diff changeset
101 //---- Make it half-quadrant : 0 .. 45 deg -------------------------------
heinrichsweikamp
parents:
diff changeset
102 ratio = (x > y) ? udiv(y, x) : udiv(x, y);
heinrichsweikamp
parents:
diff changeset
103
heinrichsweikamp
parents:
diff changeset
104 //---- Then apply the polynomial approximation ---------------------------
heinrichsweikamp
parents:
diff changeset
105 angle = umul(K1, ratio); // r*K1 / 2**16
heinrichsweikamp
parents:
diff changeset
106 x2 = umul(ratio, ratio); // r**2 / 2**16
heinrichsweikamp
parents:
diff changeset
107 x3 = umul(x2, ratio); // r**3 / 2**32
heinrichsweikamp
parents:
diff changeset
108 angle -= umul(x3, K2); // K2*r**3 / 2**48: NEGATIV.
heinrichsweikamp
parents:
diff changeset
109
heinrichsweikamp
parents:
diff changeset
110 x3 = umul(x3, x2); // r**5 / 2**64
heinrichsweikamp
parents:
diff changeset
111 angle += umul(x3, K3); // K3*r**5 / 2**80
heinrichsweikamp
parents:
diff changeset
112
heinrichsweikamp
parents:
diff changeset
113 //---- Recover the full quadrant -----------------------------------------
heinrichsweikamp
parents:
diff changeset
114 return (x < y) ? (Angle)(Q_PIO2 - angle)
heinrichsweikamp
parents:
diff changeset
115 : (Angle)(angle);
heinrichsweikamp
parents:
diff changeset
116 }
heinrichsweikamp
parents:
diff changeset
117
heinrichsweikamp
parents:
diff changeset
118 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
119 /// Computes atan2(y/x) in Angle, for x, y in range -32768 to 32767
heinrichsweikamp
parents:
diff changeset
120 ///
heinrichsweikamp
parents:
diff changeset
121 /// Results a four quadrant Angle, in the range -Q_PI .. +Q_PI
heinrichsweikamp
parents:
diff changeset
122 Angle itan(PARAMETER Int16 y, PARAMETER Int16 x)
heinrichsweikamp
parents:
diff changeset
123 {
heinrichsweikamp
parents:
diff changeset
124 // Beware: -32768 is not properly handled (sgn error).
heinrichsweikamp
parents:
diff changeset
125 if( x == -32768 ) x = -32767;
heinrichsweikamp
parents:
diff changeset
126 if( y == -32768 ) y = -32767;
heinrichsweikamp
parents:
diff changeset
127
heinrichsweikamp
parents:
diff changeset
128 if( x >= 0 )
heinrichsweikamp
parents:
diff changeset
129 if( y >= 0 ) // First quadrant: 0..90 deg.
heinrichsweikamp
parents:
diff changeset
130 return utan(y,x);
heinrichsweikamp
parents:
diff changeset
131 else // Fourth quadrant: 0..-90 deg
heinrichsweikamp
parents:
diff changeset
132 return -utan(-y,x);
heinrichsweikamp
parents:
diff changeset
133 else
heinrichsweikamp
parents:
diff changeset
134 if( y >= 0 ) // Second quadrant: 90..180 deg
heinrichsweikamp
parents:
diff changeset
135 return Q_PI - utan(y, -x);
heinrichsweikamp
parents:
diff changeset
136 else // Third quadrant: -90..-180 deg;
heinrichsweikamp
parents:
diff changeset
137 return -Q_PI + utan(-y, -x);
heinrichsweikamp
parents:
diff changeset
138 }
heinrichsweikamp
parents:
diff changeset
139
heinrichsweikamp
parents:
diff changeset
140 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
141 /// Computes cos(theta) = sqrtf(x2/h2),
heinrichsweikamp
parents:
diff changeset
142 /// when theta = atan(y/x) and h2=x*x+y*y
heinrichsweikamp
parents:
diff changeset
143 ///
heinrichsweikamp
parents:
diff changeset
144 Int16 cosxh(PARAMETER Int16 x2, PARAMETER Int16 h2)
heinrichsweikamp
parents:
diff changeset
145 {
heinrichsweikamp
parents:
diff changeset
146 OVERLAY Int16 r = 0;
heinrichsweikamp
parents:
diff changeset
147 OVERLAY Int16 d = 0x4000;
heinrichsweikamp
parents:
diff changeset
148
heinrichsweikamp
parents:
diff changeset
149 do {
heinrichsweikamp
parents:
diff changeset
150 OVERLAY Int16 a = r + d;
heinrichsweikamp
parents:
diff changeset
151 a = umul(a, a);
heinrichsweikamp
parents:
diff changeset
152 a = umul(a, h2);
heinrichsweikamp
parents:
diff changeset
153 if( a <= x2 ) r += d;
heinrichsweikamp
parents:
diff changeset
154 d >>= 1;
heinrichsweikamp
parents:
diff changeset
155 } while( d );
heinrichsweikamp
parents:
diff changeset
156
heinrichsweikamp
parents:
diff changeset
157 return r;
heinrichsweikamp
parents:
diff changeset
158 }
heinrichsweikamp
parents:
diff changeset
159
heinrichsweikamp
parents:
diff changeset
160 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
161 /// Computes both sin and cos of angle y/x,
heinrichsweikamp
parents:
diff changeset
162 /// with h = sqrt(x**2+y**2).
heinrichsweikamp
parents:
diff changeset
163 ///
heinrichsweikamp
parents:
diff changeset
164 void sincos(PARAMETER Int16 x, PARAMETER Int16 y, Int16* sin, Int16* cos)
heinrichsweikamp
parents:
diff changeset
165 {
heinrichsweikamp
parents:
diff changeset
166 OVERLAY Int16 x2, y2, h2;
heinrichsweikamp
parents:
diff changeset
167
heinrichsweikamp
parents:
diff changeset
168 //---- Fold into one quadant ---------------------------------------------
heinrichsweikamp
parents:
diff changeset
169 OVERLAY char neg = 0;
heinrichsweikamp
parents:
diff changeset
170 if( x < 0 )
heinrichsweikamp
parents:
diff changeset
171 {
heinrichsweikamp
parents:
diff changeset
172 neg |= 1;
heinrichsweikamp
parents:
diff changeset
173 x = -x;
heinrichsweikamp
parents:
diff changeset
174 }
heinrichsweikamp
parents:
diff changeset
175 if( y < 0 )
heinrichsweikamp
parents:
diff changeset
176 {
heinrichsweikamp
parents:
diff changeset
177 neg |= 2;
heinrichsweikamp
parents:
diff changeset
178 y = -y;
heinrichsweikamp
parents:
diff changeset
179 }
heinrichsweikamp
parents:
diff changeset
180
heinrichsweikamp
parents:
diff changeset
181 //---- Pre-scale both numerator and denominator ----------------------
heinrichsweikamp
parents:
diff changeset
182 while( (((x>>8) | (y>>8)) & 0xE0) == 0 )
heinrichsweikamp
parents:
diff changeset
183 {
heinrichsweikamp
parents:
diff changeset
184 x <<= 1;
heinrichsweikamp
parents:
diff changeset
185 y <<= 1;
heinrichsweikamp
parents:
diff changeset
186 }
heinrichsweikamp
parents:
diff changeset
187
heinrichsweikamp
parents:
diff changeset
188 //---- Uses trig() to do the stuff one on quadrant -------------------
heinrichsweikamp
parents:
diff changeset
189 x2 = umul(x,x);
heinrichsweikamp
parents:
diff changeset
190 y2 = umul(y,y);
heinrichsweikamp
parents:
diff changeset
191 h2 = x2 + y2;
heinrichsweikamp
parents:
diff changeset
192 x2 = cosxh(x2, h2);
heinrichsweikamp
parents:
diff changeset
193
heinrichsweikamp
parents:
diff changeset
194 //---- Results back in four quadrants --------------------------------
heinrichsweikamp
parents:
diff changeset
195 *cos = (neg & 1) ? -x2 : x2;
heinrichsweikamp
parents:
diff changeset
196 y2 = cosxh(y2, h2);
heinrichsweikamp
parents:
diff changeset
197 *sin = (neg & 2) ? -y2 : y2;
heinrichsweikamp
parents:
diff changeset
198 }
heinrichsweikamp
parents:
diff changeset
199
heinrichsweikamp
parents:
diff changeset
200 //////////////////////////////////////////////////////////////////////////////
heinrichsweikamp
parents:
diff changeset
201 //
heinrichsweikamp
parents:
diff changeset
202
heinrichsweikamp
parents:
diff changeset
203 void compass(void)
heinrichsweikamp
parents:
diff changeset
204 {
heinrichsweikamp
parents:
diff changeset
205 OVERLAY Int16 sin, cos;
heinrichsweikamp
parents:
diff changeset
206 OVERLAY Int16 iBfx, iBfy, Gz;
heinrichsweikamp
parents:
diff changeset
207 OVERLAY Int16 iBpx, iBpy, iBpz;
heinrichsweikamp
parents:
diff changeset
208 RESET_C_STACK;
heinrichsweikamp
parents:
diff changeset
209
heinrichsweikamp
parents:
diff changeset
210 //---- Make hard iron correction -----------------------------------------
heinrichsweikamp
parents:
diff changeset
211 // Measured magnetometer orientation, measured ok.
heinrichsweikamp
parents:
diff changeset
212 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.
heinrichsweikamp
parents:
diff changeset
213 iBpx = compass_DX_f - compass_CX_f; // X
heinrichsweikamp
parents:
diff changeset
214 iBpy = compass_DY_f - compass_CY_f; // Y
heinrichsweikamp
parents:
diff changeset
215 iBpz = compass_DZ_f - compass_CZ_f; // Z
heinrichsweikamp
parents:
diff changeset
216
heinrichsweikamp
parents:
diff changeset
217 //---- Calculate sine and cosine of roll angle Phi -----------------------
heinrichsweikamp
parents:
diff changeset
218 sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
heinrichsweikamp
parents:
diff changeset
219
heinrichsweikamp
parents:
diff changeset
220 //---- rotate by roll angle (-Phi) ---------------------------------------
heinrichsweikamp
parents:
diff changeset
221 iBfy = imul(iBpy, cos) - imul(iBpz, sin);
heinrichsweikamp
parents:
diff changeset
222 iBpz = imul(iBpy, sin) + imul(iBpz, cos);
heinrichsweikamp
parents:
diff changeset
223 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
heinrichsweikamp
parents:
diff changeset
224
heinrichsweikamp
parents:
diff changeset
225 //---- calculate sin and cosine of pitch angle Theta ---------------------
heinrichsweikamp
parents:
diff changeset
226 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
heinrichsweikamp
parents:
diff changeset
227
heinrichsweikamp
parents:
diff changeset
228 /* correct cosine if pitch not in range -90 to 90 degrees */
heinrichsweikamp
parents:
diff changeset
229 if( cos < 0 ) cos = -cos;
heinrichsweikamp
parents:
diff changeset
230
heinrichsweikamp
parents:
diff changeset
231 ///---- de-rotate by pitch angle Theta -----------------------------------
heinrichsweikamp
parents:
diff changeset
232 iBfx = imul(iBpx, cos) + imul(iBpz, sin);
heinrichsweikamp
parents:
diff changeset
233
heinrichsweikamp
parents:
diff changeset
234 //---- Detect uncalibrated compass ---------------------------------------
heinrichsweikamp
parents:
diff changeset
235 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
heinrichsweikamp
parents:
diff changeset
236 {
heinrichsweikamp
parents:
diff changeset
237 compass_heading = -1;
heinrichsweikamp
parents:
diff changeset
238 return;
heinrichsweikamp
parents:
diff changeset
239 }
heinrichsweikamp
parents:
diff changeset
240
heinrichsweikamp
parents:
diff changeset
241 //---- calculate current yaw = e-compass angle Psi -----------------------
heinrichsweikamp
parents:
diff changeset
242 // Result in degree (no need of 0.01 deg precision...
heinrichsweikamp
parents:
diff changeset
243 compass_heading = itan(-iBfy, iBfx) / 100;
heinrichsweikamp
parents:
diff changeset
244
heinrichsweikamp
parents:
diff changeset
245 // Result in 0..360 range:
heinrichsweikamp
parents:
diff changeset
246 if( compass_heading < 0 )
heinrichsweikamp
parents:
diff changeset
247 compass_heading += 360;
heinrichsweikamp
parents:
diff changeset
248 }