|
0
|
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
|
