|
147
|
1 #include "ostc3.inc"
|
|
|
2 #include "i2c.inc"
|
|
|
3 #include "tft_outputs.inc"
|
|
|
4 #include "isr.inc"
|
|
|
5 #include "tft.inc"
|
|
|
6 #include "strings.inc"
|
|
|
7 #include "wait.inc" ; speed_*
|
|
|
8 #include "surfmode.inc"
|
|
|
9
|
|
0
|
10
|
|
|
11 ; Make sure symbols from the .inc are available to the C code:
|
|
|
12 ; Filtered data
|
|
|
13 global compass_DX_f, compass_DY_f, compass_DZ_f
|
|
|
14 global accel_DX_f, accel_DY_f, accel_DZ_f
|
|
|
15
|
|
|
16 ; Calibration data
|
|
|
17 global compass_CX_f
|
|
|
18 global compass_CY_f
|
|
|
19 global compass_CZ_f
|
|
|
20
|
|
|
21 ; Tmp values to pass Q15 arithmetics around
|
|
|
22 global compass_a
|
|
|
23 global compass_b
|
|
|
24
|
|
|
25 ; Result
|
|
214
|
26 global compass_heading; , compass_roll, compass_pitch
|
|
0
|
27
|
|
147
|
28 extern compass
|
|
|
29 extern compass_reset_calibration
|
|
|
30 extern compass_add_calibration
|
|
|
31 extern compass_solve_calibration
|
|
|
32
|
|
0
|
33 compass code
|
|
|
34 ;-----------------------------------------------------------------------------
|
|
|
35 ; Filter compass values
|
|
|
36 ;
|
|
|
37 ; Apply linear filtering to input parameters.
|
|
|
38
|
|
|
39 ; Apply filtering formula:
|
|
|
40 ; reg_f += (reg - reg_f) / 4
|
|
|
41 FILTER16 MACRO reg, reg_f
|
|
|
42 movf reg_f+0,W
|
|
|
43 subwf reg+0,W
|
|
|
44 movwf PRODL
|
|
|
45 movf reg_f+1,W
|
|
|
46 subwfb reg+1,W
|
|
148
|
47 rcall filter_16_common
|
|
|
48 addwf reg_f+0,F
|
|
|
49 movf PRODH,W
|
|
|
50 addwfc reg_f+1,F
|
|
|
51 ENDM
|
|
|
52
|
|
|
53 filter_16_common:
|
|
0
|
54 movwf PRODH
|
|
|
55
|
|
|
56 bcf STATUS,C ; Copy sign bit into carry
|
|
|
57 btfsc PRODH,7
|
|
|
58 bsf STATUS,C
|
|
|
59 rrcf PRODH,F ; 16bit shift right
|
|
|
60 rrcf PRODL,F
|
|
|
61
|
|
|
62 bcf STATUS,C ; Copy sign bit into carry
|
|
|
63 btfsc PRODH,7
|
|
|
64 bsf STATUS,C
|
|
|
65 rrcf PRODH,F ; 16bit shift right
|
|
|
66 rrcf PRODL,W
|
|
148
|
67 return
|
|
0
|
68
|
|
147
|
69 global compass_filter
|
|
0
|
70 compass_filter:
|
|
|
71 banksel compass_DX
|
|
|
72
|
|
|
73 FILTER16 compass_DX, compass_DX_f
|
|
|
74 FILTER16 compass_DY, compass_DY_f
|
|
|
75 FILTER16 compass_DZ, compass_DZ_f
|
|
|
76 FILTER16 accel_DX, accel_DX_f
|
|
|
77 FILTER16 accel_DY, accel_DY_f
|
|
|
78 FILTER16 accel_DZ, accel_DZ_f
|
|
|
79 banksel common
|
|
|
80 return
|
|
|
81
|
|
|
82 ;-----------------------------------------------------------------------------
|
|
|
83
|
|
|
84 compass_filter_init:
|
|
|
85 movff compass_DX+0, compass_DX_f+0
|
|
|
86 movff compass_DX+1, compass_DX_f+1
|
|
|
87 movff compass_DY+0, compass_DY_f+0
|
|
|
88 movff compass_DY+1, compass_DY_f+1
|
|
|
89 movff compass_DZ+0, compass_DZ_f+0
|
|
|
90 movff compass_DZ+1, compass_DZ_f+1
|
|
|
91 movff accel_DX+0, accel_DX_f+0
|
|
|
92 movff accel_DX+1, accel_DX_f+1
|
|
|
93 movff accel_DY+0, accel_DY_f+0
|
|
|
94 movff accel_DY+1, accel_DY_f+1
|
|
|
95 movff accel_DZ+0, accel_DZ_f+0
|
|
|
96 movff accel_DZ+1, accel_DZ_f+1
|
|
|
97 return
|
|
|
98
|
|
|
99 ;-----------------------------------------------------------------------------
|
|
|
100 ; Q15 fractional numbers: a * b / 2**16 (UNSIGNED)
|
|
|
101 ;
|
|
|
102 ; Uses 16x16->16 multiply, for positiv integers, keeping only the most
|
|
|
103 ; revelant bits.
|
|
|
104 ;
|
|
|
105 ; Used to multiply two Q15 numbers, in the range 0..1,
|
|
|
106 ; represented as 0..32767, that is a / 2**15.
|
|
|
107 ;
|
|
|
108 ; (a/2**15) * (b/2**15) = a*b / 2**30 = (a*b/2**16) / 2**14.
|
|
|
109 ; So to get back a Q15 number, we need a shift-left...
|
|
|
110 global compass_umul
|
|
|
111 compass_umul:
|
|
|
112 rcall compass_mul_16
|
|
|
113
|
|
|
114 ; The 2x time, by left-shifting inserting the missing bit:
|
|
|
115 compass_mul_2:
|
|
|
116 rlcf compass_r+2,F ; Missing bit into carry
|
|
|
117 rlcf compass_r+0,F
|
|
|
118 rlcf compass_r+1,F
|
|
|
119 movff compass_r+0,PRODL ; return value into ProdH:L
|
|
|
120 movff compass_r+1,PRODH
|
|
|
121 return
|
|
|
122
|
|
|
123 ; The 16x16-> multiply:
|
|
|
124 compass_mul_16:
|
|
|
125 banksel compass_a
|
|
|
126
|
|
|
127 movf compass_a+1,W ; Block ah*bh
|
|
|
128 mulwf compass_b+1
|
|
|
129 movff PRODL,compass_r+0 ; and copy
|
|
|
130 movff PRODH,compass_r+1
|
|
|
131
|
|
|
132 movf compass_a+0,W ; Block al*bl
|
|
|
133 mulwf compass_b+0
|
|
|
134 movff PRODH,compass_r+2 ; Into fraction byte
|
|
|
135
|
|
|
136 movf compass_a+1,W ; Block ah*bl
|
|
|
137 mulwf compass_b+0
|
|
|
138 movf PRODL,W
|
|
|
139 addwf compass_r+2,F ; Fraction part to carry.
|
|
|
140 movf PRODH,W ; and add16
|
|
|
141 addwfc compass_r+0,F
|
|
|
142 movlw 0
|
|
|
143 addwfc compass_r+1,F
|
|
|
144
|
|
|
145 movf compass_a+0,W ; Block al*bh
|
|
|
146 mulwf compass_b+1
|
|
|
147 movf PRODL,W
|
|
|
148 addwf compass_r+2,F ; Fraction part to carry.
|
|
|
149 movf PRODH,W ; and add16
|
|
|
150 addwfc compass_r+0,F
|
|
|
151 movlw 0
|
|
|
152 addwfc compass_r+1,F
|
|
|
153
|
|
|
154 return
|
|
|
155
|
|
|
156 ;-----------------------------------------------------------------------------
|
|
|
157 ; Q15 fractional numbers: a * b / 2**16 (SIGNED)
|
|
|
158
|
|
|
159 global compass_imul
|
|
|
160 compass_imul:
|
|
|
161 rcall compass_mul_16
|
|
|
162
|
|
|
163 btfss compass_b+1,7
|
|
|
164 bra compass_mul_3
|
|
|
165
|
|
|
166 movf compass_a+0,W
|
|
|
167 subwf compass_r+0,F
|
|
|
168 movf compass_a+1,W
|
|
|
169 subwfb compass_r+1,F
|
|
|
170
|
|
|
171 compass_mul_3:
|
|
|
172 btfss compass_a+1,7
|
|
|
173 bra compass_mul_4
|
|
|
174
|
|
|
175 movf compass_b+0,W
|
|
|
176 subwf compass_r+0,F
|
|
|
177 movf compass_b+1,W
|
|
|
178 subwfb compass_r+1,F
|
|
|
179
|
|
|
180 compass_mul_4:
|
|
|
181 bcf compass_r+1,6 ; Copy bit 7 to 6, so keep it after 2x
|
|
|
182 btfsc compass_r+1,7
|
|
|
183 bsf compass_r+1,6
|
|
|
184 bra compass_mul_2
|
|
|
185
|
|
147
|
186 global compass_calibration_loop
|
|
|
187 compass_calibration_loop: ; Compass calibration
|
|
|
188 bsf no_sensor_int ; No Sensor ISR
|
|
|
189 call I2C_sleep_accelerometer ; Stop accelerometer
|
|
|
190 call I2C_sleep_compass ; Stop compass
|
|
|
191 call TFT_ClearScreen
|
|
|
192 ; Mask
|
|
|
193 WIN_COLOR color_greenish
|
|
|
194 WIN_SMALL .16,.0
|
|
|
195 STRCPY_TEXT_PRINT tCompassMenu
|
|
|
196 btfss switch_right2 ; wait until button is released
|
|
|
197 bra $-4
|
|
|
198
|
|
|
199 call TFT_standard_color
|
|
|
200 ; WIN_SMALL .0,.215
|
|
|
201 ; STRCPY_TEXT_PRINT tExit
|
|
|
202 WAITMS d'255'
|
|
|
203 WAITMS d'255'
|
|
|
204 movlw .7 ; Gain init
|
|
|
205 movff WREG,opt_compass_gain
|
|
|
206 compass_calibration_gainset: ; Reduce the gain, set bank here!
|
|
|
207 banksel opt_compass_gain
|
|
|
208 decf opt_compass_gain,F ; Reduce by one
|
|
|
209 btfsc STATUS,N ; <0?
|
|
|
210 clrf opt_compass_gain ; Yes, keep at zero
|
|
|
211
|
|
|
212 banksel common
|
|
|
213 call I2C_init_accelerometer
|
|
|
214 call I2C_init_compass_fast
|
|
|
215 call TFT_compass_show_gain ; Show the current compass gain
|
|
|
216
|
|
|
217 WAITMS d'100'
|
|
|
218
|
|
|
219 clrf timeout_counter2
|
|
|
220 clrf timeout_counter3
|
|
|
221
|
|
|
222 call speed_fastest
|
|
|
223 call I2C_RX_compass ; read compass
|
|
|
224 call I2C_RX_accelerometer ; read Accelerometer
|
|
|
225
|
|
|
226 ; Test all axes for +4096 (Hi byte=16)
|
|
|
227 banksel compass_DX+1
|
|
|
228 movlw .16
|
|
|
229 cpfseq compass_DX+1
|
|
|
230 bra $+4
|
|
|
231 bra compass_calibration_gainset
|
|
|
232 cpfseq compass_DY+1
|
|
|
233 bra $+4
|
|
|
234 bra compass_calibration_gainset
|
|
|
235 cpfseq compass_DZ+1
|
|
|
236 bra $+4
|
|
|
237 bra compass_calibration_gainset
|
|
|
238
|
|
|
239 ; Test all axes for -4096 (Hi byte=240)
|
|
|
240 movlw .240
|
|
|
241 cpfseq compass_DX+1
|
|
|
242 bra $+4
|
|
|
243 bra compass_calibration_gainset
|
|
|
244 cpfseq compass_DY+1
|
|
|
245 bra $+4
|
|
|
246 bra compass_calibration_gainset
|
|
|
247 cpfseq compass_DZ+1
|
|
|
248 bra $+4
|
|
|
249 bra compass_calibration_gainset
|
|
|
250 banksel common
|
|
|
251
|
|
|
252 rcall compass_filter_init ; set DX_f values
|
|
|
253 call compass_reset_calibration ; Reset CX_f values
|
|
|
254 banksel common
|
|
|
255
|
|
|
256 compass_calibration_loop2:
|
|
|
257 call I2C_RX_compass ; read compass
|
|
|
258 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
259 rcall compass_filter ; Filter compass raw data
|
|
|
260 banksel common
|
|
|
261
|
|
|
262 ; Twice
|
|
|
263 call I2C_RX_compass ; read compass
|
|
|
264 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
265 rcall compass_filter ; Filter compass raw data
|
|
|
266 banksel common
|
|
|
267
|
|
|
268 ; Test all axes for +4096 (Hi byte=16)
|
|
|
269 banksel compass_DX+1
|
|
|
270 movlw .16
|
|
|
271 cpfseq compass_DX+1
|
|
|
272 bra $+4
|
|
|
273 bra compass_calibration_gainset
|
|
|
274 cpfseq compass_DY+1
|
|
|
275 bra $+4
|
|
|
276 bra compass_calibration_gainset
|
|
|
277 cpfseq compass_DZ+1
|
|
|
278 bra $+4
|
|
|
279 bra compass_calibration_gainset
|
|
|
280
|
|
|
281 ; Test all axes for -4096 (Hi byte=240)
|
|
|
282 movlw .240
|
|
|
283 cpfseq compass_DX+1
|
|
|
284 bra $+4
|
|
|
285 bra compass_calibration_gainset
|
|
|
286 cpfseq compass_DY+1
|
|
|
287 bra $+4
|
|
|
288 bra compass_calibration_gainset
|
|
|
289 cpfseq compass_DZ+1
|
|
|
290 bra $+4
|
|
|
291 bra compass_calibration_gainset
|
|
|
292 banksel common
|
|
|
293 ;
|
|
|
294 ; ; Three
|
|
|
295 ; call I2C_RX_compass ; read compass
|
|
|
296 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
297 ; call compass_filter ; Filter compass raw data
|
|
|
298 ; banksel common
|
|
|
299 ;
|
|
|
300 ; ; Four times to get cleaner values
|
|
|
301 ; call I2C_RX_compass ; read compass
|
|
|
302 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
303 ; call compass_filter ; Filter compass raw data
|
|
|
304
|
|
|
305 ; And register only one value out of four:
|
|
|
306 call compass_add_calibration ; check and store new max/min values
|
|
|
307 banksel common
|
|
|
308
|
|
|
309 call TFT_compass_fast ; show values
|
|
|
310
|
|
|
311 btfsc sleepmode ; Sleepmode active?
|
|
|
312 bra compass_calibration_exit ; Yes, exit
|
|
|
313
|
|
|
314 ; btfsc switch_left ; Button pressed?
|
|
|
315 ; bra compass_calibration_exit ; Yes, exit
|
|
|
316
|
|
|
317 btfss onesecupdate ; do every second tasks?
|
|
|
318 bra compass_calibration_loop2 ; no, loop here
|
|
|
319
|
|
|
320 movlw .60
|
|
|
321 call timeout_testmode ; check timeout
|
|
|
322 movlw .60
|
|
|
323 call TFT_show_timeout_testmode ; Show the timeout
|
|
|
324
|
|
|
325 bcf onesecupdate ; clear flag
|
|
|
326
|
|
|
327 bra compass_calibration_loop2 ; loop here
|
|
|
328
|
|
|
329 compass_calibration_exit:
|
|
|
330 call compass_solve_calibration
|
|
|
331 banksel common
|
|
|
332 extern option_save_all
|
|
|
333 call option_save_all ; save all settings into EEPROM
|
|
|
334 bcf sleepmode ; Clear the flag before exiting to surfacemode
|
|
|
335 movlw .6
|
|
|
336 movwf customview_surfmode ; Set to compass view...
|
|
|
337 goto surfloop ; ...and exit
|
|
|
338
|
|
|
339
|
|
0
|
340 END
|
