Mercurial > public > hwos_code

comparison src/compass_ops.asm @ 147:fdd4e30846ae

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
some cleanup
author heinrichsweikamp
date Wed, 06 Aug 2014 11:59:04 +0200
parents 11d4fc797f74
children 022b886eddaf
comparison
equal deleted inserted replaced
146:d6ad414c7c12 147:fdd4e30846ae
1 #include "ostc3.inc" 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
2 10
3 ; Make sure symbols from the .inc are available to the C code: 11 ; Make sure symbols from the .inc are available to the C code:
4 ; Filtered data 12 ; Filtered data
5 global compass_DX_f, compass_DY_f, compass_DZ_f 13 global compass_DX_f, compass_DY_f, compass_DZ_f
6 global accel_DX_f, accel_DY_f, accel_DZ_f 14 global accel_DX_f, accel_DY_f, accel_DZ_f
15 global compass_b 23 global compass_b
16 24
17 ; Result 25 ; Result
18 global compass_heading, compass_roll, compass_pitch 26 global compass_heading, compass_roll, compass_pitch
19 27
28 extern compass
29 extern compass_reset_calibration
30 extern compass_add_calibration
31 extern compass_solve_calibration
32
20 compass code 33 compass code
21 ;----------------------------------------------------------------------------- 34 ;-----------------------------------------------------------------------------
22 ; Filter compass values 35 ; Filter compass values
23 ; 36 ;
24 ; Apply linear filtering to input parameters. 37 ; Apply linear filtering to input parameters.
25 global compass_filter
26 38
27 ; Apply filtering formula: 39 ; Apply filtering formula:
28 ; reg_f += (reg - reg_f) / 4 40 ; reg_f += (reg - reg_f) / 4
29 FILTER16 MACRO reg, reg_f 41 FILTER16 MACRO reg, reg_f
30 movf reg_f+0,W 42 movf reg_f+0,W
49 addwf reg_f+0,F 61 addwf reg_f+0,F
50 movf PRODH,W 62 movf PRODH,W
51 addwfc reg_f+1,F 63 addwfc reg_f+1,F
52 ENDM 64 ENDM
53 65
66 global compass_filter
54 compass_filter: 67 compass_filter:
55 banksel compass_DX 68 banksel compass_DX
56 69
57 FILTER16 compass_DX, compass_DX_f 70 FILTER16 compass_DX, compass_DX_f
58 FILTER16 compass_DY, compass_DY_f 71 FILTER16 compass_DY, compass_DY_f
63 banksel common 76 banksel common
64 return 77 return
65 78
66 ;----------------------------------------------------------------------------- 79 ;-----------------------------------------------------------------------------
67 80
68 global compass_filter_init
69 compass_filter_init: 81 compass_filter_init:
70 movff compass_DX+0, compass_DX_f+0 82 movff compass_DX+0, compass_DX_f+0
71 movff compass_DX+1, compass_DX_f+1 83 movff compass_DX+1, compass_DX_f+1
72 movff compass_DY+0, compass_DY_f+0 84 movff compass_DY+0, compass_DY_f+0
73 movff compass_DY+1, compass_DY_f+1 85 movff compass_DY+1, compass_DY_f+1
166 bcf compass_r+1,6 ; Copy bit 7 to 6, so keep it after 2x 178 bcf compass_r+1,6 ; Copy bit 7 to 6, so keep it after 2x
167 btfsc compass_r+1,7 179 btfsc compass_r+1,7
168 bsf compass_r+1,6 180 bsf compass_r+1,6
169 bra compass_mul_2 181 bra compass_mul_2
170 182
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
171 END 337 END