Mercurial > public > hwos_code

comparison src/compass.c @ 650:bc214815deb2

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
3.19/10.75 release
author heinrichsweikamp
date Sun, 28 Aug 2022 13:13:38 +0200
parents cd58f7fc86db
children 75e90cd0c2c3
comparison
equal deleted inserted replaced
649:ef2ed7e3a895 650:bc214815deb2
16 16
17 #ifdef _compass 17 #ifdef _compass
18 18
19 19
20 ////////////////////////////////////////////////////////////////////////////// 20 //////////////////////////////////////////////////////////////////////////////
21 // mH: crude work-around, needs to be fixed up 21 //
22 // Put compass data into bank 13 (stack) and bank 9 (variables)
23 //
22 #ifndef UNIX 24 #ifndef UNIX
23 # pragma udata overlay bank8=0x800 25 # pragma udata overlay bank13=0xd00
24 static char C_STACK[256]; // overlay C-code data stack here 26 static char C_STACK[256]; // overlay C-code data stack here
25 # define RESET_C_STACK \ 27 # define C_STACK_ADDR C_STACK
26 _asm \ 28 # define RESET_C_STACK \
27 LFSR 1, 0x800 \ 29 _asm \
28 LFSR 2, 0x800 \ 30 LFSR 1,C_STACK_ADDR \
31 LFSR 2,C_STACK_ADDR \
29 _endasm 32 _endasm
30 # pragma udata overlay bank9_compass 33 # pragma udata bank9a = 0x900
31 # pragma code compass_run 34 # pragma code compass_run
35 # define PARAMETER static
36 # define OVERLAY overlay
32 #else 37 #else
33 # define RESET_C_STACK 38 # define RESET_C_STACK
39 # define PARAMETER
40 # define OVERLAY
34 #endif 41 #endif
42
35 43
36 ////////////////////////////////////////////////////////////////////////////// 44 //////////////////////////////////////////////////////////////////////////////
37 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error 45 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
38 // 46 //
39 #define K1 (5701) // needs K1/2**16 47 #define K1 (5701) // needs K1/2**16
245 iBpy = compass_DY_f - compass_CY_f; // Y 253 iBpy = compass_DY_f - compass_CY_f; // Y
246 iBpz = compass_DZ_f - compass_CZ_f; // Z 254 iBpz = compass_DZ_f - compass_CZ_f; // Z
247 255
248 //---- Calculate sine and cosine of roll angle Phi ----------------------- 256 //---- Calculate sine and cosine of roll angle Phi -----------------------
249 sincos(accel_DZ_f, accel_DY_f, &sin, &cos); 257 sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
250 258
251 //---- rotate by roll angle (-Phi) --------------------------------------- 259 //---- rotate by roll angle (-Phi) ---------------------------------------
252 iBfy = imul(iBpy, cos) - imul(iBpz, sin); 260 iBfy = imul(iBpy, cos) - imul(iBpz, sin);
253 iBpz = imul(iBpy, sin) + imul(iBpz, cos); 261 iBpz = imul(iBpy, sin) + imul(iBpz, cos);
254 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos); 262 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
255 263