Mercurial > public > hwos_code

comparison src/compass.c @ 604:ca4556fb60b9

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