Mercurial > public > hwos_code
diff src/compass.c @ 282:7d9edd3b8c86
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 |
line wrap: on
line diff
--- a/src/compass.c Tue May 26 17:40:13 2015 +0200 +++ b/src/compass.c Fri May 22 14:50:40 2015 +0200 @@ -1,8 +1,13 @@ +////////////////////////////////////////////////////////////////////////////// +/// compass.c +/// Compute north direction from magnetic/acceleration measures. +/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved. ////////////////////////////////////////////////////////////////////////////// // HISTORY // 2012-12-01 [jDG] Creation // 2012-12-23 [jDG] Added filtering. // 2012-12-30 [jDG] Added calibration (spherical best fit). +// 2015-05-22 [jDG] Minor cleanups. Smaller calibration code. #include "compass.h" @@ -17,8 +22,9 @@ LFSR 2, 0x800 \ _endasm # pragma udata overlay bank9_compass +# pragma code compass_run #else -# define RESET_C_STACK +# define RESET_C_STACK #endif ////////////////////////////////////////////////////////////////////////////// @@ -210,7 +216,6 @@ //---- Calculate sine and cosine of roll angle Phi ----------------------- sincos(accel_DZ_f, accel_DY_f, &sin, &cos); -// compass_roll = itan(sin, cos) / 100; //---- rotate by roll angle (-Phi) --------------------------------------- iBfy = imul(iBpy, cos) - imul(iBpz, sin); @@ -219,7 +224,6 @@ //---- calculate sin and cosine of pitch angle Theta --------------------- sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign. -// compass_pitch = itan(sin, cos) / 100; /* correct cosine if pitch not in range -90 to 90 degrees */ if( cos < 0 ) cos = -cos;