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