Mercurial > public > hwos_code

comparison src/compass.c @ 0:11d4fc797f74

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