Mercurial > public > hwos_code

comparison src/compass.c @ 282:7d9edd3b8c86

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Make a more compact COMPASS calibration code (<7KB), and add more tests.
author jDG
date Fri, 22 May 2015 14:50:40 +0200
parents a17359244d93
children ca4556fb60b9
comparison
equal deleted inserted replaced
281:eb758a5b44eb 282:7d9edd3b8c86
1 //////////////////////////////////////////////////////////////////////////////
2 /// compass.c
3 /// Compute north direction from magnetic/acceleration measures.
4 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
1 ////////////////////////////////////////////////////////////////////////////// 5 //////////////////////////////////////////////////////////////////////////////
2 // HISTORY 6 // HISTORY
3 // 2012-12-01 [jDG] Creation 7 // 2012-12-01 [jDG] Creation
4 // 2012-12-23 [jDG] Added filtering. 8 // 2012-12-23 [jDG] Added filtering.
5 // 2012-12-30 [jDG] Added calibration (spherical best fit). 9 // 2012-12-30 [jDG] Added calibration (spherical best fit).
10 // 2015-05-22 [jDG] Minor cleanups. Smaller calibration code.
6 11
7 #include "compass.h" 12 #include "compass.h"
8 13
9 ////////////////////////////////////////////////////////////////////////////// 14 //////////////////////////////////////////////////////////////////////////////
10 // mH: Crude work-around, needs to be made right 15 // mH: Crude work-around, needs to be made right
15 _asm \ 20 _asm \
16 LFSR 1, 0x800 \ 21 LFSR 1, 0x800 \
17 LFSR 2, 0x800 \ 22 LFSR 2, 0x800 \
18 _endasm 23 _endasm
19 # pragma udata overlay bank9_compass 24 # pragma udata overlay bank9_compass
25 # pragma code compass_run
20 #else 26 #else
21 # define RESET_C_STACK 27 # define RESET_C_STACK
22 #endif 28 #endif
23 29
24 ////////////////////////////////////////////////////////////////////////////// 30 //////////////////////////////////////////////////////////////////////////////
25 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error 31 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
26 // 32 //
208 iBpy = compass_DY_f - compass_CY_f; // Y 214 iBpy = compass_DY_f - compass_CY_f; // Y
209 iBpz = compass_DZ_f - compass_CZ_f; // Z 215 iBpz = compass_DZ_f - compass_CZ_f; // Z
210 216
211 //---- Calculate sine and cosine of roll angle Phi ----------------------- 217 //---- Calculate sine and cosine of roll angle Phi -----------------------
212 sincos(accel_DZ_f, accel_DY_f, &sin, &cos); 218 sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
213 // compass_roll = itan(sin, cos) / 100;
214 219
215 //---- rotate by roll angle (-Phi) --------------------------------------- 220 //---- rotate by roll angle (-Phi) ---------------------------------------
216 iBfy = imul(iBpy, cos) - imul(iBpz, sin); 221 iBfy = imul(iBpy, cos) - imul(iBpz, sin);
217 iBpz = imul(iBpy, sin) + imul(iBpz, cos); 222 iBpz = imul(iBpy, sin) + imul(iBpz, cos);
218 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos); 223 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
219 224
220 //---- calculate sin and cosine of pitch angle Theta --------------------- 225 //---- calculate sin and cosine of pitch angle Theta ---------------------
221 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign. 226 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
222 // compass_pitch = itan(sin, cos) / 100;
223 227
224 /* correct cosine if pitch not in range -90 to 90 degrees */ 228 /* correct cosine if pitch not in range -90 to 90 degrees */
225 if( cos < 0 ) cos = -cos; 229 if( cos < 0 ) cos = -cos;
226 230
227 ///---- de-rotate by pitch angle Theta ----------------------------------- 231 ///---- de-rotate by pitch angle Theta -----------------------------------