annotate src/compass_ops.asm @ 46:05227510cae0

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