Mercurial > public > hwos_code

comparison src/compass.c @ 623:c40025d8e750

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
3.03 beta released
author heinrichsweikamp
date Mon, 03 Jun 2019 14:01:48 +0200
parents 1ad0531e9078
children cd58f7fc86db
comparison
equal deleted inserted replaced
622:02d1386429a6 623:c40025d8e750
1 ////////////////////////////////////////////////////////////////////////////// 1 //////////////////////////////////////////////////////////////////////////////
2 /// compass.c 2 /// compass.c
3 /// Compute north direction from magnetic/acceleration measures 3 /// Compute north direction from magnetic/acceleration measures V3.02.1
4 /// 4 ///
5 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved. 5 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
6 ////////////////////////////////////////////////////////////////////////////// 6 //////////////////////////////////////////////////////////////////////////////
7 // HISTORY 7 // HISTORY
8 // 2012-12-01 [jDG] Creation 8 // 2012-12-01 [jDG] Creation
9 // 2012-12-23 [jDG] Added filtering. 9 // 2012-12-23 [jDG] Added filtering.
10 // 2012-12-30 [jDG] Added calibration (spherical best fit). 10 // 2012-12-30 [jDG] Added calibration (spherical best fit).
11 // 2015-05-22 [jDG] Minor cleanups. Smaller calibration code. 11 // 2015-05-22 [jDG] Minor cleanups. Smaller calibration code.
12 12
13 #include "configuration.inc"
13 #include "compass.h" 14 #include "compass.h"
15
16
17 #ifdef _compass
18
14 19
15 ////////////////////////////////////////////////////////////////////////////// 20 //////////////////////////////////////////////////////////////////////////////
16 // mH: crude work-around, needs to be fixed up 21 // mH: crude work-around, needs to be fixed up
17 #ifndef UNIX 22 #ifndef UNIX
18 # pragma udata overlay bank8=0x800 23 # pragma udata overlay bank8=0x800
65 /// represented as 0..32767. 70 /// represented as 0..32767.
66 71
67 Int16 udiv(PARAMETER Int16 a, PARAMETER Int16 b) 72 Int16 udiv(PARAMETER Int16 a, PARAMETER Int16 b)
68 { 73 {
69 OVERLAY Int16 d, r; 74 OVERLAY Int16 d, r;
70 OVERLAY char failsafe=250; 75 OVERLAY char failsafe=250;
71 76
72 //---- Pre-scale both numerator and denominator -------------------------- 77 //---- Pre-scale both numerator and denominator --------------------------
73 while( (((a>>8) | (b>>8)) & 0xC0) == 0 ) 78 while( (((a>>8) | (b>>8)) & 0xC0) == 0 )
74 { 79 {
75 failsafe--; 80 failsafe--;
76 if (failsafe==0) break; 81 if( failsafe == 0 ) break;
82
77 a <<= 1; 83 a <<= 1;
78 b <<= 1; 84 b <<= 1;
79 } 85 }
80 86
81 //---- Make division trials ---------------------------------------------- 87 //---- Make division trials ----------------------------------------------
174 /// 180 ///
175 181
176 void sincos(PARAMETER Int16 x, PARAMETER Int16 y, Int16* sin, Int16* cos) 182 void sincos(PARAMETER Int16 x, PARAMETER Int16 y, Int16* sin, Int16* cos)
177 { 183 {
178 OVERLAY Int16 x2, y2, h2; 184 OVERLAY Int16 x2, y2, h2;
179 OVERLAY char failsafe=250; 185 OVERLAY char failsafe = 250;
180 186
181 //---- Fold into one quadant --------------------------------------------- 187 //---- Fold into one quadrant --------------------------------------------
182 OVERLAY char neg = 0; 188 OVERLAY char neg = 0;
183 if( x < 0 ) 189 if( x < 0 )
184 { 190 {
185 neg |= 1; 191 neg |= 1;
186 x = -x; 192 x = -x;
188 if( y < 0 ) 194 if( y < 0 )
189 { 195 {
190 neg |= 2; 196 neg |= 2;
191 y = -y; 197 y = -y;
192 } 198 }
193 199
194 //---- Pre-scale both numerator and denominator ---------------------- 200 //---- Pre-scale both numerator and denominator ----------------------
195 while( (((x>>8) | (y>>8)) & 0xE0) == 0 ) 201 while( (((x>>8) | (y>>8)) & 0xE0) == 0 )
196 { 202 {
197 failsafe--; 203 failsafe--;
198 if (failsafe==0) break; 204 if( failsafe == 0 ) break;
205
199 x <<= 1; 206 x <<= 1;
200 y <<= 1; 207 y <<= 1;
201 } 208 }
202 209
203 //---- Uses trig() to do the stuff one on quadrant ------------------- 210 //---- Uses trig() to do the stuff one on quadrant -------------------
220 OVERLAY Int16 sin, cos; 227 OVERLAY Int16 sin, cos;
221 OVERLAY Int16 iBfx, iBfy, Gz; 228 OVERLAY Int16 iBfx, iBfy, Gz;
222 OVERLAY Int16 iBpx, iBpy, iBpz; 229 OVERLAY Int16 iBpx, iBpy, iBpz;
223 230
224 RESET_C_STACK; 231 RESET_C_STACK;
232
233 //---- Detect uncalibrated compass ---------------------------------------
234 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
235 {
236 // no usable compass is signaled by bit 15 set to 1
237 compass_heading_new = 32768;
238 return;
239 }
225 240
226 //---- Make hard iron correction ----------------------------------------- 241 //---- Make hard iron correction -----------------------------------------
227 // Measured magnetometer orientation, measured ok 242 // Measured magnetometer orientation, measured ok
228 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation 243 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation
229 iBpx = compass_DX_f - compass_CX_f; // X 244 iBpx = compass_DX_f - compass_CX_f; // X
245 if( cos < 0 ) cos = -cos; 260 if( cos < 0 ) cos = -cos;
246 261
247 ///---- de-rotate by pitch angle Theta ----------------------------------- 262 ///---- de-rotate by pitch angle Theta -----------------------------------
248 iBfx = imul(iBpx, cos) + imul(iBpz, sin); 263 iBfx = imul(iBpx, cos) + imul(iBpz, sin);
249 264
250 //---- Detect uncalibrated compass ---------------------------------------
251 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
252 {
253 compass_heading = -1;
254 return;
255 }
256
257 //---- calculate current yaw = e-compass angle Psi ----------------------- 265 //---- calculate current yaw = e-compass angle Psi -----------------------
258 // Result in degree (no need of 0.01 deg precision... 266 // Result in degree (no need of 0.01 deg precision...
259 compass_heading = itan(-iBfy, iBfx) / 100; 267 compass_heading_new = itan(-iBfy, iBfx) / 100;
260 268
261 // Result in 0..360 range: 269 // Result in 0..360 range:
262 if( compass_heading < 0 ) 270 if( compass_heading_new < 0 )
263 compass_heading += 360; 271 compass_heading_new += 360;
264 } 272 }
273
274 #endif // _compass