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