Mercurial > public > hwos_code

comparison src/compass_ops.asm @ 0:11d4fc797f74

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
init
author heinrichsweikamp
date Wed, 24 Apr 2013 19:22:45 +0200
parents
children fdd4e30846ae
comparison
equal deleted inserted replaced
-1:000000000000 0:11d4fc797f74
1 #include "ostc3.inc"
2
3 ; Make sure symbols from the .inc are available to the C code:
4 ; Filtered data
5 global compass_DX_f, compass_DY_f, compass_DZ_f
6 global accel_DX_f, accel_DY_f, accel_DZ_f
7
8 ; Calibration data
9 global compass_CX_f
10 global compass_CY_f
11 global compass_CZ_f
12
13 ; Tmp values to pass Q15 arithmetics around
14 global compass_a
15 global compass_b
16
17 ; Result
18 global compass_heading, compass_roll, compass_pitch
19
20 compass code
21 ;-----------------------------------------------------------------------------
22 ; Filter compass values
23 ;
24 ; Apply linear filtering to input parameters.
25 global compass_filter
26
27 ; Apply filtering formula:
28 ; reg_f += (reg - reg_f) / 4
29 FILTER16 MACRO reg, reg_f
30 movf reg_f+0,W
31 subwf reg+0,W
32 movwf PRODL
33 movf reg_f+1,W
34 subwfb reg+1,W
35 movwf PRODH
36
37 bcf STATUS,C ; Copy sign bit into carry
38 btfsc PRODH,7
39 bsf STATUS,C
40 rrcf PRODH,F ; 16bit shift right
41 rrcf PRODL,F
42
43 bcf STATUS,C ; Copy sign bit into carry
44 btfsc PRODH,7
45 bsf STATUS,C
46 rrcf PRODH,F ; 16bit shift right
47 rrcf PRODL,W
48
49 addwf reg_f+0,F
50 movf PRODH,W
51 addwfc reg_f+1,F
52 ENDM
53
54 compass_filter:
55 banksel compass_DX
56
57 FILTER16 compass_DX, compass_DX_f
58 FILTER16 compass_DY, compass_DY_f
59 FILTER16 compass_DZ, compass_DZ_f
60 FILTER16 accel_DX, accel_DX_f
61 FILTER16 accel_DY, accel_DY_f
62 FILTER16 accel_DZ, accel_DZ_f
63 banksel common
64 return
65
66 ;-----------------------------------------------------------------------------
67
68 global compass_filter_init
69 compass_filter_init:
70 movff compass_DX+0, compass_DX_f+0
71 movff compass_DX+1, compass_DX_f+1
72 movff compass_DY+0, compass_DY_f+0
73 movff compass_DY+1, compass_DY_f+1
74 movff compass_DZ+0, compass_DZ_f+0
75 movff compass_DZ+1, compass_DZ_f+1
76 movff accel_DX+0, accel_DX_f+0
77 movff accel_DX+1, accel_DX_f+1
78 movff accel_DY+0, accel_DY_f+0
79 movff accel_DY+1, accel_DY_f+1
80 movff accel_DZ+0, accel_DZ_f+0
81 movff accel_DZ+1, accel_DZ_f+1
82 return
83
84 ;-----------------------------------------------------------------------------
85 ; Q15 fractional numbers: a * b / 2**16 (UNSIGNED)
86 ;
87 ; Uses 16x16->16 multiply, for positiv integers, keeping only the most
88 ; revelant bits.
89 ;
90 ; Used to multiply two Q15 numbers, in the range 0..1,
91 ; represented as 0..32767, that is a / 2**15.
92 ;
93 ; (a/2**15) * (b/2**15) = a*b / 2**30 = (a*b/2**16) / 2**14.
94 ; So to get back a Q15 number, we need a shift-left...
95 global compass_umul
96 compass_umul:
97 rcall compass_mul_16
98
99 ; The 2x time, by left-shifting inserting the missing bit:
100 compass_mul_2:
101 rlcf compass_r+2,F ; Missing bit into carry
102 rlcf compass_r+0,F
103 rlcf compass_r+1,F
104 movff compass_r+0,PRODL ; return value into ProdH:L
105 movff compass_r+1,PRODH
106 return
107
108 ; The 16x16-> multiply:
109 compass_mul_16:
110 banksel compass_a
111
112 movf compass_a+1,W ; Block ah*bh
113 mulwf compass_b+1
114 movff PRODL,compass_r+0 ; and copy
115 movff PRODH,compass_r+1
116
117 movf compass_a+0,W ; Block al*bl
118 mulwf compass_b+0
119 movff PRODH,compass_r+2 ; Into fraction byte
120
121 movf compass_a+1,W ; Block ah*bl
122 mulwf compass_b+0
123 movf PRODL,W
124 addwf compass_r+2,F ; Fraction part to carry.
125 movf PRODH,W ; and add16
126 addwfc compass_r+0,F
127 movlw 0
128 addwfc compass_r+1,F
129
130 movf compass_a+0,W ; Block al*bh
131 mulwf compass_b+1
132 movf PRODL,W
133 addwf compass_r+2,F ; Fraction part to carry.
134 movf PRODH,W ; and add16
135 addwfc compass_r+0,F
136 movlw 0
137 addwfc compass_r+1,F
138
139 return
140
141 ;-----------------------------------------------------------------------------
142 ; Q15 fractional numbers: a * b / 2**16 (SIGNED)
143
144 global compass_imul
145 compass_imul:
146 rcall compass_mul_16
147
148 btfss compass_b+1,7
149 bra compass_mul_3
150
151 movf compass_a+0,W
152 subwf compass_r+0,F
153 movf compass_a+1,W
154 subwfb compass_r+1,F
155
156 compass_mul_3:
157 btfss compass_a+1,7
158 bra compass_mul_4
159
160 movf compass_b+0,W
161 subwf compass_r+0,F
162 movf compass_b+1,W
163 subwfb compass_r+1,F
164
165 compass_mul_4:
166 bcf compass_r+1,6 ; Copy bit 7 to 6, so keep it after 2x
167 btfsc compass_r+1,7
168 bsf compass_r+1,6
169 bra compass_mul_2
170
171 END