275
|
1 #include "hwos.inc"
|
147
|
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"
|
256
|
9 #include "divemode.inc"
|
|
10 #include "math.inc"
|
|
11 #include "convert.inc"
|
147
|
12
|
0
|
13
|
|
14 ; Make sure symbols from the .inc are available to the C code:
|
|
15 ; Filtered data
|
|
16 global compass_DX_f, compass_DY_f, compass_DZ_f
|
|
17 global accel_DX_f, accel_DY_f, accel_DZ_f
|
|
18
|
|
19 ; Calibration data
|
|
20 global compass_CX_f
|
|
21 global compass_CY_f
|
|
22 global compass_CZ_f
|
|
23
|
|
24 ; Tmp values to pass Q15 arithmetics around
|
|
25 global compass_a
|
|
26 global compass_b
|
|
27
|
|
28 ; Result
|
214
|
29 global compass_heading; , compass_roll, compass_pitch
|
0
|
30
|
147
|
31 extern compass
|
|
32 extern compass_reset_calibration
|
|
33 extern compass_add_calibration
|
|
34 extern compass_solve_calibration
|
|
35
|
337
508d7fb98b34
cleanup menu, add "Auto SP" option (Not working yet), minor layout change in compass menu
heinrichsweikamp
diff
changeset
|
36 extern menu_processor_bottom_line
|
508d7fb98b34
cleanup menu, add "Auto SP" option (Not working yet), minor layout change in compass menu
heinrichsweikamp
diff
changeset
|
37
|
282
|
38 compass_ops code
|
0
|
39 ;-----------------------------------------------------------------------------
|
|
40 ; Filter compass values
|
|
41 ;
|
|
42 ; Apply linear filtering to input parameters.
|
|
43
|
|
44 ; Apply filtering formula:
|
|
45 ; reg_f += (reg - reg_f) / 4
|
|
46 FILTER16 MACRO reg, reg_f
|
|
47 movf reg_f+0,W
|
|
48 subwf reg+0,W
|
|
49 movwf PRODL
|
|
50 movf reg_f+1,W
|
|
51 subwfb reg+1,W
|
148
|
52 rcall filter_16_common
|
|
53 addwf reg_f+0,F
|
|
54 movf PRODH,W
|
|
55 addwfc reg_f+1,F
|
|
56 ENDM
|
|
57
|
|
58 filter_16_common:
|
0
|
59 movwf PRODH
|
|
60
|
|
61 bcf STATUS,C ; Copy sign bit into carry
|
|
62 btfsc PRODH,7
|
|
63 bsf STATUS,C
|
|
64 rrcf PRODH,F ; 16bit shift right
|
|
65 rrcf PRODL,F
|
|
66
|
|
67 bcf STATUS,C ; Copy sign bit into carry
|
|
68 btfsc PRODH,7
|
|
69 bsf STATUS,C
|
|
70 rrcf PRODH,F ; 16bit shift right
|
|
71 rrcf PRODL,W
|
148
|
72 return
|
0
|
73
|
147
|
74 global compass_filter
|
0
|
75 compass_filter:
|
|
76 banksel compass_DX
|
|
77
|
|
78 FILTER16 compass_DX, compass_DX_f
|
|
79 FILTER16 compass_DY, compass_DY_f
|
|
80 FILTER16 compass_DZ, compass_DZ_f
|
|
81 FILTER16 accel_DX, accel_DX_f
|
|
82 FILTER16 accel_DY, accel_DY_f
|
|
83 FILTER16 accel_DZ, accel_DZ_f
|
|
84 banksel common
|
|
85 return
|
|
86
|
|
87 ;-----------------------------------------------------------------------------
|
|
88
|
|
89 compass_filter_init:
|
|
90 movff compass_DX+0, compass_DX_f+0
|
|
91 movff compass_DX+1, compass_DX_f+1
|
|
92 movff compass_DY+0, compass_DY_f+0
|
|
93 movff compass_DY+1, compass_DY_f+1
|
|
94 movff compass_DZ+0, compass_DZ_f+0
|
|
95 movff compass_DZ+1, compass_DZ_f+1
|
|
96 movff accel_DX+0, accel_DX_f+0
|
|
97 movff accel_DX+1, accel_DX_f+1
|
|
98 movff accel_DY+0, accel_DY_f+0
|
|
99 movff accel_DY+1, accel_DY_f+1
|
|
100 movff accel_DZ+0, accel_DZ_f+0
|
|
101 movff accel_DZ+1, accel_DZ_f+1
|
|
102 return
|
|
103
|
|
104 ;-----------------------------------------------------------------------------
|
|
105 ; Q15 fractional numbers: a * b / 2**16 (UNSIGNED)
|
|
106 ;
|
|
107 ; Uses 16x16->16 multiply, for positiv integers, keeping only the most
|
|
108 ; revelant bits.
|
|
109 ;
|
|
110 ; Used to multiply two Q15 numbers, in the range 0..1,
|
|
111 ; represented as 0..32767, that is a / 2**15.
|
|
112 ;
|
|
113 ; (a/2**15) * (b/2**15) = a*b / 2**30 = (a*b/2**16) / 2**14.
|
|
114 ; So to get back a Q15 number, we need a shift-left...
|
|
115 global compass_umul
|
|
116 compass_umul:
|
|
117 rcall compass_mul_16
|
|
118
|
|
119 ; The 2x time, by left-shifting inserting the missing bit:
|
|
120 compass_mul_2:
|
|
121 rlcf compass_r+2,F ; Missing bit into carry
|
|
122 rlcf compass_r+0,F
|
|
123 rlcf compass_r+1,F
|
|
124 movff compass_r+0,PRODL ; return value into ProdH:L
|
|
125 movff compass_r+1,PRODH
|
|
126 return
|
|
127
|
|
128 ; The 16x16-> multiply:
|
|
129 compass_mul_16:
|
|
130 banksel compass_a
|
|
131
|
|
132 movf compass_a+1,W ; Block ah*bh
|
|
133 mulwf compass_b+1
|
|
134 movff PRODL,compass_r+0 ; and copy
|
|
135 movff PRODH,compass_r+1
|
|
136
|
|
137 movf compass_a+0,W ; Block al*bl
|
|
138 mulwf compass_b+0
|
|
139 movff PRODH,compass_r+2 ; Into fraction byte
|
|
140
|
|
141 movf compass_a+1,W ; Block ah*bl
|
|
142 mulwf compass_b+0
|
|
143 movf PRODL,W
|
|
144 addwf compass_r+2,F ; Fraction part to carry.
|
|
145 movf PRODH,W ; and add16
|
|
146 addwfc compass_r+0,F
|
|
147 movlw 0
|
|
148 addwfc compass_r+1,F
|
|
149
|
|
150 movf compass_a+0,W ; Block al*bh
|
|
151 mulwf compass_b+1
|
|
152 movf PRODL,W
|
|
153 addwf compass_r+2,F ; Fraction part to carry.
|
|
154 movf PRODH,W ; and add16
|
|
155 addwfc compass_r+0,F
|
|
156 movlw 0
|
|
157 addwfc compass_r+1,F
|
|
158
|
|
159 return
|
|
160
|
|
161 ;-----------------------------------------------------------------------------
|
|
162 ; Q15 fractional numbers: a * b / 2**16 (SIGNED)
|
|
163
|
|
164 global compass_imul
|
|
165 compass_imul:
|
|
166 rcall compass_mul_16
|
|
167
|
|
168 btfss compass_b+1,7
|
|
169 bra compass_mul_3
|
|
170
|
|
171 movf compass_a+0,W
|
|
172 subwf compass_r+0,F
|
|
173 movf compass_a+1,W
|
|
174 subwfb compass_r+1,F
|
|
175
|
|
176 compass_mul_3:
|
|
177 btfss compass_a+1,7
|
|
178 bra compass_mul_4
|
|
179
|
|
180 movf compass_b+0,W
|
|
181 subwf compass_r+0,F
|
|
182 movf compass_b+1,W
|
|
183 subwfb compass_r+1,F
|
|
184
|
|
185 compass_mul_4:
|
|
186 bcf compass_r+1,6 ; Copy bit 7 to 6, so keep it after 2x
|
|
187 btfsc compass_r+1,7
|
|
188 bsf compass_r+1,6
|
|
189 bra compass_mul_2
|
|
190
|
147
|
191 global compass_calibration_loop
|
|
192 compass_calibration_loop: ; Compass calibration
|
|
193 bsf no_sensor_int ; No Sensor ISR
|
|
194 call I2C_sleep_accelerometer ; Stop accelerometer
|
|
195 call I2C_sleep_compass ; Stop compass
|
|
196 call TFT_ClearScreen
|
|
197 ; Mask
|
|
198 WIN_COLOR color_greenish
|
|
199 WIN_SMALL .16,.0
|
|
200 STRCPY_TEXT_PRINT tCompassMenu
|
428
|
201 btfss switch_right2 ; wait until button is released
|
147
|
202 bra $-4
|
|
203
|
|
204 call TFT_standard_color
|
|
205 ; WIN_SMALL .0,.215
|
|
206 ; STRCPY_TEXT_PRINT tExit
|
|
207 WAITMS d'255'
|
|
208 WAITMS d'255'
|
428
|
209
|
147
|
210 movlw .7 ; Gain init
|
|
211 movff WREG,opt_compass_gain
|
|
212 compass_calibration_gainset: ; Reduce the gain, set bank here!
|
|
213 banksel opt_compass_gain
|
|
214 decf opt_compass_gain,F ; Reduce by one
|
|
215 btfsc STATUS,N ; <0?
|
|
216 clrf opt_compass_gain ; Yes, keep at zero
|
|
217
|
|
218 banksel common
|
|
219 call I2C_init_accelerometer
|
|
220 call I2C_init_compass_fast
|
428
|
221
|
|
222 btfsc compass_type ; compass1?
|
|
223 bra compass_calibration_loop1 ; Yes, skip gain stuff
|
|
224
|
|
225 ; rcall TFT_compass_show_gain ; Show the current compass gain
|
147
|
226
|
|
227 WAITMS d'100'
|
|
228
|
|
229 clrf timeout_counter2
|
|
230 clrf timeout_counter3
|
|
231
|
|
232 call speed_fastest
|
|
233 call I2C_RX_compass ; read compass
|
|
234 call I2C_RX_accelerometer ; read Accelerometer
|
|
235
|
|
236 ; Test all axes for +4096 (Hi byte=16)
|
|
237 banksel compass_DX+1
|
|
238 movlw .16
|
|
239 cpfseq compass_DX+1
|
|
240 bra $+4
|
|
241 bra compass_calibration_gainset
|
|
242 cpfseq compass_DY+1
|
|
243 bra $+4
|
|
244 bra compass_calibration_gainset
|
|
245 cpfseq compass_DZ+1
|
|
246 bra $+4
|
|
247 bra compass_calibration_gainset
|
|
248
|
|
249 ; Test all axes for -4096 (Hi byte=240)
|
|
250 movlw .240
|
|
251 cpfseq compass_DX+1
|
|
252 bra $+4
|
|
253 bra compass_calibration_gainset
|
|
254 cpfseq compass_DY+1
|
|
255 bra $+4
|
|
256 bra compass_calibration_gainset
|
|
257 cpfseq compass_DZ+1
|
|
258 bra $+4
|
|
259 bra compass_calibration_gainset
|
|
260 banksel common
|
|
261
|
428
|
262 compass_calibration_loop1: ; Done with Gain
|
147
|
263 rcall compass_filter_init ; set DX_f values
|
|
264 call compass_reset_calibration ; Reset CX_f values
|
|
265 banksel common
|
|
266
|
|
267 compass_calibration_loop2:
|
|
268 call I2C_RX_compass ; read compass
|
|
269 call I2C_RX_accelerometer ; Test Accelerometer
|
|
270 rcall compass_filter ; Filter compass raw data
|
|
271 banksel common
|
|
272
|
|
273 ; Twice
|
|
274 call I2C_RX_compass ; read compass
|
|
275 call I2C_RX_accelerometer ; Test Accelerometer
|
|
276 rcall compass_filter ; Filter compass raw data
|
|
277 banksel common
|
428
|
278
|
|
279 btfsc compass_type ; compass1?
|
|
280 bra compass_calibration_loop3 ; Yes, skip gain stuff
|
147
|
281
|
|
282 ; Test all axes for +4096 (Hi byte=16)
|
|
283 banksel compass_DX+1
|
|
284 movlw .16
|
|
285 cpfseq compass_DX+1
|
|
286 bra $+4
|
|
287 bra compass_calibration_gainset
|
|
288 cpfseq compass_DY+1
|
|
289 bra $+4
|
|
290 bra compass_calibration_gainset
|
|
291 cpfseq compass_DZ+1
|
|
292 bra $+4
|
|
293 bra compass_calibration_gainset
|
|
294
|
|
295 ; Test all axes for -4096 (Hi byte=240)
|
|
296 movlw .240
|
|
297 cpfseq compass_DX+1
|
|
298 bra $+4
|
|
299 bra compass_calibration_gainset
|
|
300 cpfseq compass_DY+1
|
|
301 bra $+4
|
|
302 bra compass_calibration_gainset
|
|
303 cpfseq compass_DZ+1
|
|
304 bra $+4
|
|
305 bra compass_calibration_gainset
|
|
306 banksel common
|
|
307 ;
|
|
308 ; ; Three
|
|
309 ; call I2C_RX_compass ; read compass
|
|
310 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
311 ; call compass_filter ; Filter compass raw data
|
|
312 ; banksel common
|
|
313 ;
|
|
314 ; ; Four times to get cleaner values
|
|
315 ; call I2C_RX_compass ; read compass
|
|
316 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
317 ; call compass_filter ; Filter compass raw data
|
|
318
|
428
|
319 compass_calibration_loop3:
|
147
|
320 ; And register only one value out of four:
|
|
321 call compass_add_calibration ; check and store new max/min values
|
|
322 banksel common
|
|
323
|
|
324 call TFT_compass_fast ; show values
|
|
325
|
|
326 btfsc sleepmode ; Sleepmode active?
|
|
327 bra compass_calibration_exit ; Yes, exit
|
|
328
|
|
329 ; btfsc switch_left ; Button pressed?
|
|
330 ; bra compass_calibration_exit ; Yes, exit
|
|
331
|
|
332 btfss onesecupdate ; do every second tasks?
|
|
333 bra compass_calibration_loop2 ; no, loop here
|
|
334
|
|
335 movlw .60
|
|
336 call timeout_testmode ; check timeout
|
|
337 movlw .60
|
256
|
338 rcall TFT_show_timeout_testmode ; Show the timeout
|
147
|
339
|
|
340 bcf onesecupdate ; clear flag
|
|
341
|
|
342 bra compass_calibration_loop2 ; loop here
|
|
343
|
|
344 compass_calibration_exit:
|
|
345 call compass_solve_calibration
|
|
346 banksel common
|
|
347 extern option_save_all
|
|
348 call option_save_all ; save all settings into EEPROM
|
|
349 bcf sleepmode ; Clear the flag before exiting to surfacemode
|
|
350 movlw .6
|
|
351 movwf customview_surfmode ; Set to compass view...
|
|
352 goto surfloop ; ...and exit
|
|
353
|
256
|
354 global TFT_compass_fast
|
|
355 TFT_compass_fast:
|
|
356 WIN_TINY .20,.50
|
|
357 STRCPY "X:"
|
|
358 movff compass_DX+0,lo
|
|
359 movff compass_DX+1,hi
|
|
360 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
|
|
361 output_16
|
|
362 STRCAT " Y:"
|
|
363 movff compass_DY+0,lo
|
|
364 movff compass_DY+1,hi
|
|
365 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
|
|
366 output_16
|
|
367 STRCAT " Z:"
|
|
368 movff compass_DZ+0,lo
|
|
369 movff compass_DZ+1,hi
|
|
370 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
|
|
371 output_16
|
|
372 STRCAT_PRINT " "
|
|
373 return
|
|
374
|
|
375 TFT_show_timeout_testmode: ; With timeout in WREG...
|
|
376 movwf hi
|
|
377 WIN_TINY .20,.68
|
|
378 STRCPY "T:"
|
|
379 movf timeout_counter2,W ; current timeout
|
|
380 subwf hi,W ; subtract from timeout value
|
|
381 addlw .1 ; +1
|
|
382 movwf lo
|
|
383 bsf leftbind
|
|
384 output_8 ; Display timeout
|
|
385 bcf leftbind
|
|
386 STRCAT_PRINT "s "
|
|
387 return
|
|
388
|
|
389
|
|
390 TFT_compass_show_gain: ; Show the current compass gain
|
|
391 movff opt_compass_gain,lo ; 0-7 (230LSB/Gauss to 1370LSB/Gaus)
|
|
392 tstfsz lo
|
|
393 return ; Do not show unless gain=0
|
|
394 WIN_TINY .20,.86
|
|
395 STRCPY_TEXT tCompassGain
|
|
396 movff opt_compass_gain,lo ; 0-7 (230LSB/Gauss to 1370LSB/Gaus)
|
|
397 bsf leftbind
|
|
398 output_8
|
|
399 bcf leftbind
|
|
400 STRCAT_PRINT "!"
|
|
401 return
|
|
402
|
382
|
403 TFT_surface_compass_bearing:
|
|
404 WIN_SMALL surf_compass_bear_column,surf_compass_bear_row
|
|
405 movff compass_bearing+0,lo
|
|
406 movff compass_bearing+1,hi
|
|
407 PUTC "("
|
|
408 bsf leftbind
|
|
409 output_16dp .2 ; Result is "0.000"
|
|
410 bcf leftbind
|
|
411 ; rearrange figures to "000"
|
|
412 movff buffer+3,buffer+1
|
|
413 movff buffer+4,buffer+2
|
|
414 movff buffer+5,buffer+3
|
|
415 lfsr FSR2,buffer+4
|
|
416 STRCAT "° "
|
|
417 rcall tft_compass_cardinal ; Add cardinal and ordinal to POSTINC2
|
|
418 STRCAT_PRINT ")"
|
|
419 return
|
|
420
|
256
|
421 global TFT_surface_compass_mask
|
|
422 TFT_surface_compass_mask:
|
|
423 WIN_SMALL surf_compass_mask_column,surf_compass_mask_row
|
|
424 call TFT_standard_color
|
|
425 STRCPY_TEXT_PRINT tHeading ; Heading:
|
|
426 return
|
|
427
|
|
428 global TFT_dive_compass_mask
|
|
429 TFT_dive_compass_mask:
|
300
|
430 WIN_FRAME_STD dm_custom_compass_graph_row, dm_custom_compass_graph_row+dm_custom_compass_graph_height, .0, .159
|
256
|
431 return
|
|
432
|
|
433 global TFT_surface_compass_heading
|
|
434 TFT_surface_compass_heading:
|
|
435 rcall compass_heading_common
|
|
436 WIN_STD surf_compass_head_column,surf_compass_head_row
|
|
437 call TFT_standard_color
|
|
438 TFT_surface_compass_heading_com: ; Show "000° N"
|
|
439 movff compass_heading+0,lo
|
|
440 movff compass_heading+1,hi
|
|
441 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
|
|
442
|
|
443 ; Shown and actual identical?
|
|
444 movff compass_heading_shown+0,WREG
|
|
445 cpfseq lo
|
|
446 bra TFT_surface_compass_heading_com1 ; Not equal
|
|
447 movff compass_heading_shown+1,WREG
|
|
448 cpfseq hi
|
|
449 bra TFT_surface_compass_heading_com1 ; Not equal
|
|
450 bra TFT_surface_compass_heading_com3 ; equal, skip smoothing
|
|
451
|
|
452 TFT_surface_compass_heading_com1:
|
|
453 movff lo,sub_a+0
|
|
454 movff hi,sub_a+1
|
|
455 movff compass_heading_shown+0,sub_b+0
|
|
456 movff compass_heading_shown+1,sub_b+1
|
|
457 call subU16
|
|
458 btfsc neg_flag
|
|
459 bra TFT_surface_compass_heading_com2 ; shown > actual
|
|
460 ; shown < actual
|
|
461 banksel compass_heading_shown
|
|
462 infsnz compass_heading_shown+0,F
|
|
463 incf compass_heading_shown+1,F ; +1
|
|
464 bra TFT_surface_compass_heading_com3
|
|
465
|
|
466 TFT_surface_compass_heading_com2:
|
|
467 banksel compass_heading_shown
|
|
468 movlw d'1'
|
432
|
469 subwf compass_heading_shown+0,F
|
|
470 movlw d'0'
|
|
471 subwfb compass_heading_shown+1,F ; -1
|
256
|
472
|
|
473 TFT_surface_compass_heading_com3:
|
|
474 banksel common
|
|
475 movff compass_heading_shown+0,lo
|
|
476 movff compass_heading_shown+1,hi
|
|
477 bsf leftbind
|
|
478 output_16dp .2 ; Result is "0.000"
|
|
479 bcf leftbind
|
|
480 ; rearrange figures to "000"
|
|
481 movff buffer+2,buffer+0
|
|
482 movff buffer+3,buffer+1
|
|
483 movff buffer+4,buffer+2
|
|
484 lfsr FSR2,buffer+3
|
|
485 STRCAT "° "
|
|
486 rcall tft_compass_cardinal ; Add cardinal and ordinal to POSTINC2
|
|
487 clrf WREG
|
|
488 movff WREG,buffer+.7 ; limit to 7 chars
|
|
489 STRCAT_PRINT ""
|
382
|
490
|
|
491 btfsc divemode
|
|
492 return ; Done for divemode.
|
|
493 ; Show bearing on the surface?
|
|
494 btfss compass_bearing_set
|
|
495 return ; No, return
|
|
496 btfsc premenu ; "Bearing?" shown?
|
|
497 return ; Yes, return
|
|
498 bra TFT_surface_compass_bearing
|
256
|
499
|
|
500 global TFT_dive_compass_heading
|
|
501 TFT_dive_compass_heading:
|
|
502 rcall compass_heading_common
|
|
503 ; ; ToDo - these are for development only, hardcoding the bearing position
|
|
504 ; ; 244° : SW - W
|
|
505 ; movlw low(d'244')
|
|
506 ; movff WREG,compass_bearing+0
|
|
507 ; movlw high(d'244')
|
|
508 ; movff WREG,compass_bearing+1
|
|
509
|
|
510 movff compass_heading_shown+0,xA+0
|
|
511 movff compass_heading_shown+1,xA+1
|
|
512 ; xRD and xRDlft
|
|
513 ; 1. 160° viewing angle: +360 offset if xA<=292; for non-negative scale
|
|
514 movlw high(d'292')
|
|
515 movff WREG,sub_a+1
|
|
516 movlw low(d'292')
|
|
517 movff WREG,sub_a+0
|
|
518 movff xA+1,sub_b+1
|
|
519 movff xA+0,sub_b+0
|
|
520 call subU16 ; sub_c = sub_a - sub_b
|
|
521 btfsc neg_flag ; xA>292
|
|
522 bra TFT_dive_compass_heading_1 ;yes
|
|
523 ; no, xA<=292
|
|
524 movlw high(d'360')
|
|
525 addwf xA+1,1
|
|
526 movlw low(d'360')
|
|
527 addwf xA+0,1
|
|
528 btfsc STATUS,C
|
|
529 incf xA+1
|
|
530 TFT_dive_compass_heading_1:
|
|
531 ; 2. -80: left pixel offset from the center
|
|
532 movlw low( d'80' )
|
|
533 subwf xA+0,1
|
|
534 btfss STATUS,C
|
|
535 decf xA+1
|
|
536 ; 3. save it to xRD
|
|
537 movff xA+0,xRD+0
|
|
538 movff xA+1,xRD+1
|
|
539 ; 4. add 160 (display px width)
|
|
540 movlw high(d'160')
|
|
541 addwf xA+1,1
|
|
542 movlw low(d'160')
|
|
543 addwf xA+0,1
|
|
544 btfsc STATUS,C
|
|
545 incf xA+1
|
|
546 ; 5. save it to xRDr
|
|
547 movff xA+0,xRDr+0
|
|
548 movff xA+1,xRDr+1
|
|
549
|
|
550 btfss compass_bearing_set
|
|
551 bra TFT_dive_compass_ruler ; no value in the bearing, skip calc
|
|
552
|
|
553 ; we have bearing set, we will need xRD180 calculated
|
|
554 ; xRD180 is xRDr-180
|
|
555 movff xRDr+1,sub_a+1
|
|
556 movff xRDr+0,sub_a+0
|
|
557 movlw high(d'180')
|
|
558 movff WREG,sub_b+1
|
|
559 movlw low(d'180')
|
|
560 movff WREG,sub_b+0
|
|
561 call subU16 ; sub_c = sub_a - sub_b
|
|
562 movff sub_c+1,xRD180+1
|
|
563 movff sub_c+0,xRD180+0
|
|
564
|
258
|
565 TFT_dive_compass_bearing_1:
|
|
566 ; calculate bearing position and visibility (ahead or behind)
|
|
567 bcf compass_bearing_vis ; default is not-visibly
|
|
568 bcf compass_bearing_ahd ; default is behind
|
256
|
569 ; get the bearing virtual display offset, store it to divA
|
|
570 movff compass_bearing+0,xA+0
|
|
571 movff compass_bearing+1,xA+1
|
|
572 movlw high(d'360')
|
|
573 addwf xA+1,1
|
|
574 movlw low(d'360')
|
|
575 addwf xA+0,1
|
|
576 btfsc STATUS,C
|
|
577 incf xA+1
|
258
|
578 ; save it to reuse for upper/lower turns and ahead/behind checks
|
256
|
579 movff xA+1,divA+1
|
|
580 movff xA+0,divA+0
|
|
581
|
|
582 ; check if it's ahead
|
|
583 ; load the bearing offset into sub_a
|
|
584 movff divA+1,sub_a+1
|
|
585 movff divA+0,sub_a+0
|
|
586 ; load the display offset back to sub_b
|
|
587 movff xRD+0,sub_b+0
|
|
588 movff xRD+1,sub_b+1
|
|
589 rcall TFT_dive_compass_bearing_ap
|
|
590 ;test if we found it
|
|
591 btfsc compass_bearing_vis
|
|
592 bra TFT_dive_compass_bearing_dir
|
|
593
|
258
|
594 ; check if it's ahead with an upper turn
|
256
|
595 ; load the bearing offset into sub_a
|
|
596 movff divA+1,sub_a+1
|
|
597 movff divA+0,sub_a+0
|
|
598 ; load the display offset back to sub_b
|
|
599 movff xRD+0,sub_b+0
|
|
600 movff xRD+1,sub_b+1
|
|
601 movlw high(d'360')
|
|
602 addwf sub_b+1,1
|
|
603 movlw low(d'360')
|
|
604 addwf sub_b+0,1
|
|
605 btfsc STATUS,C
|
|
606 incf sub_b+1
|
|
607 rcall TFT_dive_compass_bearing_ap
|
|
608 ;test if we found it
|
|
609 btfsc compass_bearing_vis
|
|
610 bra TFT_dive_compass_bearing_dir
|
|
611
|
258
|
612 ; check if it's ahead with a lower turn
|
|
613 ; load the bearing offset into sub_a
|
|
614 movff divA+1,sub_a+1
|
|
615 movff divA+0,sub_a+0
|
|
616 movlw high(d'360')
|
|
617 addwf sub_a+1,1
|
|
618 movlw low(d'360')
|
|
619 addwf sub_a+0,1
|
|
620 btfsc STATUS,C
|
|
621 incf sub_a+1
|
|
622 ; load the display offset back to sub_b
|
|
623 movff xRD+0,sub_b+0
|
|
624 movff xRD+1,sub_b+1
|
|
625 rcall TFT_dive_compass_bearing_ap
|
|
626 ;test if we found it
|
|
627 btfsc compass_bearing_vis
|
|
628 bra TFT_dive_compass_bearing_dir
|
|
629
|
256
|
630 ; marker is not ahead of us, check if it's behind us
|
|
631 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display
|
|
632 ; load the display offset back to sub_a
|
|
633 movff xRD180+0,sub_a+0
|
|
634 movff xRD180+1,sub_a+1
|
|
635 ; load the marker's offset into sub_b
|
|
636 movff divA+0,sub_b+0
|
|
637 movff divA+1,sub_b+1
|
|
638 rcall TFT_dive_compass_bearing_bp
|
|
639 ;test if we found it
|
|
640 btfsc compass_bearing_vis
|
|
641 bra TFT_dive_compass_bearing_dir
|
|
642
|
|
643 ;check if it's behind with the lower turn
|
|
644 movff xRD180+0,sub_a+0
|
|
645 movff xRD180+1,sub_a+1
|
|
646 movlw high(d'360')
|
|
647 addwf sub_a+1,1
|
|
648 movlw low(d'360')
|
|
649 addwf sub_a+0,1
|
|
650 btfsc STATUS,C
|
|
651 incf sub_a+1
|
|
652 ; load the marker's offset into sub_b
|
|
653 movff divA+0,sub_b+0
|
|
654 movff divA+1,sub_b+1
|
|
655 rcall TFT_dive_compass_bearing_bp
|
|
656 ;test if we found it
|
|
657 btfsc compass_bearing_vis
|
|
658 bra TFT_dive_compass_bearing_dir
|
|
659
|
|
660 ; check if it's behind with the upper turn
|
|
661 movff divA+1,sub_b+1
|
|
662 movff divA+0,sub_b+0
|
|
663 movlw high(d'360')
|
|
664 addwf sub_b+1,1
|
|
665 movlw low(d'360')
|
|
666 addwf sub_b+0,1
|
|
667 btfsc STATUS,C
|
|
668 incf sub_b+1
|
|
669 rcall TFT_dive_compass_bearing_bp
|
|
670 bra TFT_dive_compass_bearing_dir
|
|
671
|
|
672 TFT_dive_compass_bearing_ap:
|
|
673 ; xCM received in sub_a
|
|
674 ; xRD received in sub_b
|
|
675 ; 1/a. check if it's viewable from the left side
|
|
676 call subU16 ; sub_c = sub_a - sub_b
|
|
677 btfsc neg_flag ; xRD>divA
|
|
678 return ;no,
|
|
679 ; yes, store the RO=RP-RD for drawing
|
|
680 movff sub_c+0,xC+0
|
|
681 movff sub_c+1,xC+1
|
|
682 ; 1/b. check if it's viewable from the right side?
|
|
683 movlw d'2' ; avoid thin mess on the side of the display
|
|
684 addwf sub_a+0,1
|
|
685 btfsc STATUS, C
|
|
686 incf sub_a+1
|
|
687 ; load the display offset right side into sub_b
|
371
|
688 movlw high(d'158')
|
256
|
689 addwf sub_b+1,1
|
371
|
690 movlw low(d'158')
|
256
|
691 addwf sub_b+0,1
|
|
692 btfsc STATUS,C
|
|
693 incf sub_b+1
|
|
694 call subU16 ; sub_c = sub_a - sub_b
|
|
695 btfss neg_flag ; xRDr>xA(+2)
|
|
696 return ; no,
|
|
697 ; print the bearing lines on the screen
|
|
698 movff xC+0,xCM
|
|
699 bsf compass_bearing_vis ; set visible
|
|
700 bsf compass_bearing_ahd ; set ahead
|
|
701 return ; done,
|
|
702
|
|
703 TFT_dive_compass_bearing_bp:
|
|
704 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display
|
|
705 ; the marker's offset received in sub_b
|
|
706 ; the xRD180 display offset received in sub_a
|
|
707 ; xRD180 - xCM
|
|
708 call subU16 ; sub_c = sub_a - sub_b
|
|
709 btfsc neg_flag ; CM>xRD180
|
|
710 return ; no, not on screen
|
|
711 ; 160 - (X)
|
371
|
712 movlw high(d'158')
|
256
|
713 movff WREG,sub_a+1
|
371
|
714 movlw low(d'158')
|
256
|
715 movff WREG,sub_a+0
|
|
716 movff sub_c+1,sub_b+1
|
|
717 movff sub_c+0,sub_b+0
|
|
718 call subU16 ; sub_c = sub_a - sub_b
|
|
719 btfsc neg_flag ; X>160
|
|
720 return ; no, not on screen
|
|
721 ; check if not overflow - this sounds a double check...
|
|
722 movlw d'1'
|
|
723 cpfslt sub_c+1
|
|
724 return ; high set, >160
|
371
|
725 movlw d'158'
|
256
|
726 cpfslt sub_c+0
|
|
727 return ; low >160
|
|
728 ; print the bearing lines on the screen
|
|
729 movff sub_c+0,xCM
|
|
730 bsf compass_bearing_vis
|
|
731 return ; done
|
|
732
|
|
733 TFT_dive_compass_bearing_dir:
|
|
734 ; check if bearing to heading, and calculate the direction
|
|
735 bcf compass_bearing_eq
|
|
736 btfss compass_bearing_vis
|
|
737 bra TFT_dive_compass_bearing_lr
|
|
738 btfss compass_bearing_ahd
|
|
739 bra TFT_dive_compass_bearing_lr
|
|
740 movff xCM,xA+0
|
|
741 movlw d'80'
|
|
742 cpfseq xA+0
|
|
743 bra TFT_dive_compass_bearing_lr
|
|
744 bsf compass_bearing_eq
|
|
745 bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler
|
|
746
|
|
747 TFT_dive_compass_bearing_lr:
|
258
|
748 ; get the bearing virtual display offset
|
|
749 movff compass_bearing+0,xA+0
|
|
750 movff compass_bearing+1,xA+1
|
|
751 ; divA =IF (U10>292;U10;U10+360)
|
|
752 movlw high(d'292')
|
|
753 movff WREG,sub_a+1
|
|
754 movlw low(d'292')
|
|
755 movff WREG,sub_a+0
|
|
756 movff xA+1,sub_b+1
|
|
757 movff xA+0,sub_b+0
|
|
758 call subU16 ; sub_c = sub_a - sub_b
|
|
759 btfsc neg_flag ; xA>292
|
|
760 bra TFT_dive_compass_bearing_lr_1 ;yes
|
|
761 ; no, xA<=292
|
|
762 movlw high(d'360')
|
|
763 addwf xA+1,1
|
|
764 movlw low(d'360')
|
|
765 addwf xA+0,1
|
|
766 btfsc STATUS,C
|
|
767 incf xA+1
|
|
768 TFT_dive_compass_bearing_lr_1:
|
256
|
769 ; 1. calculate whether bearing is to left or to right
|
|
770 bsf compass_bearing_lft ; to the left by default
|
|
771 ; xC: save center value to compare the direction to front value
|
|
772 movff xA+1,xC+1
|
|
773 movff xA+0,xC+0
|
|
774 ; xB: we need the left side for comparism... left = -180
|
|
775 movff xA+1,sub_a+1
|
|
776 movff xA+0,sub_a+0
|
|
777 movlw high(d'180')
|
|
778 movff WREG,sub_b+1
|
|
779 movlw low(d'180')
|
|
780 movff WREG,sub_b+0
|
|
781 call subU16 ; sub_c = sub_a - sub_b
|
|
782 movff sub_c+1,xB+1 ; xB has the left side of the 180° distance center
|
|
783 movff sub_c+0,xB+0
|
|
784 ; xA = IF(xRD>(xC+100);xRD-280;xRD+80)
|
|
785 movff xC+1,sub_a+1
|
|
786 movff xC+0,sub_a+0
|
|
787 movlw d'100'
|
|
788 addwf sub_a+0,1
|
|
789 btfsc STATUS,C
|
|
790 incf sub_a+1
|
|
791 movff xRD+1,sub_b+1
|
|
792 movff xRD+0,sub_b+0
|
|
793 call subU16 ; sub_c = sub_a - sub_b
|
|
794 btfsc neg_flag ; xRD>xC+100
|
|
795 bra TFT_dive_compass_bearing_lr_2 ; yes, xA=xRD-280
|
|
796 ; no, xA = xRD+80
|
|
797 movff xRD+1,xA+1
|
|
798 movff xRD+0,xA+0
|
|
799 movlw d'80'
|
|
800 addwf xA+0,1
|
|
801 btfsc STATUS,C
|
|
802 incf xA+1
|
|
803 bra TFT_dive_compass_bearing_lr_c
|
|
804
|
|
805 TFT_dive_compass_bearing_lr_2:
|
|
806 ; xA=xRD-280
|
|
807 movff xRD+1,sub_a+1
|
|
808 movff xRD+0,sub_a+0
|
|
809 movlw high(d'280')
|
|
810 movff WREG,sub_b+1
|
|
811 movlw low(d'280')
|
|
812 movff WREG,sub_b+0
|
|
813 call subU16 ; sub_c = sub_a - sub_b
|
|
814 movff sub_c+1,xA+1
|
|
815 movff sub_c+0,xA+0
|
|
816 ;bra TFT_dive_compass_bearing_lr_c
|
|
817
|
|
818 TFT_dive_compass_bearing_lr_c:
|
|
819 ; xB < xA < xC => right, otherwise left (default)
|
|
820 movff xA+1,sub_b+1
|
|
821 movff xA+0,sub_b+0
|
|
822 movff xB+1,sub_a+1
|
|
823 movff xB+0,sub_a+0
|
|
824 call subU16 ; sub_c = sub_a - sub_b
|
|
825 btfss neg_flag ; xA>xB ?
|
|
826 bra TFT_dive_compass_ruler ; No, xB >= xA, keep default left
|
|
827 movff xA+1,sub_a+1
|
|
828 movff xA+0,sub_a+0
|
|
829 movff xC+1,sub_b+1
|
|
830 movff xC+0,sub_b+0
|
|
831 call subU16 ; sub_c = sub_a - sub_b
|
|
832 btfss neg_flag ; xC>xA ?
|
|
833 bra TFT_dive_compass_ruler ; No, xA >= xC, keep default left
|
|
834 bcf compass_bearing_lft
|
|
835
|
|
836 TFT_dive_compass_ruler:
|
|
837 ; calculate mod15 for the ticks
|
|
838 movff xRD+0,xA+0
|
|
839 movff xRD+1,xA+1
|
|
840 movlw d'15'
|
|
841 movwf xB+0
|
|
842 clrf xB+1
|
|
843 call div16x16 ;xA/xB=xC with xA+0 as remainder
|
|
844 ; check xA+0, it has the remainder
|
|
845 movlw d'0'
|
|
846 cpfsgt xA+0 ; mod15 > 0
|
|
847 bra TFT_dive_compass_ruler_1 ; no, RM = 0
|
|
848 ; yes RM = 15 - RDmod15
|
|
849 movlw d'15'
|
|
850 subfwb xA+0,1
|
|
851 TFT_dive_compass_ruler_1:
|
|
852 ; xA+0 holds the RM, store it to 'lo'
|
|
853 movff xA+0,lo
|
|
854 ; init DD to zero, store it to 'hi'
|
371
|
855 clrf hi
|
256
|
856
|
|
857 TFT_dive_compass_ruler_loop:
|
|
858 ; 1. check if we run of from the display
|
371
|
859 movlw d'159' ; Looks like 159 works because TFT_box limits the dispay
|
256
|
860 cpfslt lo,1
|
|
861 bra TFT_dive_compass_ruler_lend ; xRM >= W
|
|
862 ; 2. Clear the tick area from DD to RM - in segments to avoid blinking
|
|
863 ; don't do a clear if we are at 0 (zero) otherwise it will blink
|
|
864 ; because of the width underflow
|
|
865 movlw d'0'
|
|
866 cpfsgt lo,1
|
|
867 bra TFT_dive_compass_ruler_loop_zz
|
|
868 rcall TFT_dive_compass_clr_ruler
|
|
869 TFT_dive_compass_ruler_loop_zz:
|
|
870 ; 3. Draw the markers @ RM
|
370
|
871 ; we receive RM in lo and DD in hi
|
|
872 movlw dm_custom_compass_tick_top_top
|
432
|
873 movwf win_top
|
370
|
874 movlw dm_custom_compass_tick_height
|
432
|
875 movwf win_height
|
370
|
876 movlw d'2'
|
432
|
877 movwf win_width+0
|
|
878 clrf win_width+1
|
|
879 movwf win_bargraph
|
370
|
880 movff lo,win_leftx2 ; 0..159
|
|
881 call TFT_standard_color
|
371
|
882 call TFT_box
|
370
|
883 movlw dm_custom_compass_tick_bot_top
|
432
|
884 movwf win_top
|
370
|
885 movlw dm_custom_compass_tick_height
|
432
|
886 movwf win_height
|
370
|
887 call TFT_standard_color ; color in WREG is trashed, must be set again!
|
371
|
888 call TFT_box
|
256
|
889 ; 4. If D<82 and RM>79: means we put something over the center line
|
|
890 ; redraw the center line
|
|
891 movlw d'82'
|
|
892 cpfslt hi,1
|
|
893 bra TFT_dive_compass_ruler_loop_zz2
|
|
894 movlw d'79'
|
|
895 cpfsgt lo,1
|
|
896 bra TFT_dive_compass_ruler_loop_zz2
|
370
|
897 ;enough to print cline as bearing marker is not in the ticker area
|
|
898 movlw color_yellow
|
|
899 WIN_BOX_COLOR dm_custom_compass_tick_top_top, dm_custom_compass_tick_bot_bot,.80,.81 ; center line in yellow
|
256
|
900 TFT_dive_compass_ruler_loop_zz2:
|
|
901 ; 5. set D = RM + 2 : position after the 2px tick
|
|
902 movff lo,hi
|
|
903 movlw d'2'
|
371
|
904 addwf hi,F
|
256
|
905 ; 6. set RM = RM + 15 : position to the next tick
|
|
906 movlw d'15'
|
371
|
907 addwf lo,F
|
256
|
908 ; 7. loop
|
|
909 bra TFT_dive_compass_ruler_loop
|
|
910
|
|
911 TFT_dive_compass_ruler_lend: ; loop end
|
|
912 ; 8. clear the rest of the tick area if D<160
|
|
913 movlw d'160'
|
371
|
914 cpfslt hi
|
256
|
915 bra TFT_dive_compass_ruler_lend2 ; D >= W
|
|
916 ; 9. position left to end of display to clear the remaining area
|
370
|
917 movlw d'159'
|
256
|
918 movwf lo
|
|
919 ; 10. clear it
|
|
920 rcall TFT_dive_compass_clr_ruler
|
|
921
|
|
922 TFT_dive_compass_ruler_lend2:
|
|
923 ; done with the compass ruler, put the labels on the screen
|
|
924 ; get the RD abck to sub_b
|
|
925 movff xRD+0,sub_b+0
|
|
926 movff xRD+1,sub_b+1
|
|
927 ; hi stores the display position
|
371
|
928 clrf hi
|
|
929 clrf xHI
|
256
|
930 ; lo stores the last item's display position
|
371
|
931 clrf lo
|
|
932 clrf xLO
|
256
|
933
|
|
934 movlw low( d'219' ) ; position of the label
|
|
935 movwf sub_a+0
|
|
936 movlw high( d'219' )
|
371
|
937 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
256
|
938 btfss print_compass_label ; Yes?
|
|
939 bra dcr_1
|
|
940 STRCPY_TEXT_PRINT tSW ; yes - print it
|
|
941 dcr_1:
|
|
942 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
943
|
|
944 movlw low( d'267' ) ; position of the label
|
|
945 movwf sub_a+0
|
|
946 movlw high( d'267' )
|
|
947 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
948 btfss print_compass_label ; Yes?
|
|
949 bra dcr_2
|
|
950 STRCPY_TEXT_PRINT tW ; yes - print it
|
|
951 dcr_2:
|
|
952 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
953
|
|
954 movlw low( d'309' ) ; position of the label
|
|
955 movwf sub_a+0
|
|
956 movlw high( d'309' )
|
|
957 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
958 btfss print_compass_label ; Yes?
|
|
959 bra dcr_3
|
|
960 STRCPY_TEXT_PRINT tNW ; yes - print it
|
|
961 dcr_3:
|
|
962 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
963
|
|
964 movlw low( d'358' ) ; position of the label
|
|
965 movwf sub_a+0
|
|
966 movlw high( d'358' )
|
|
967 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
968 btfss print_compass_label ; Yes?
|
|
969 bra dcr_4
|
|
970 STRCPY_TEXT_PRINT tN ; yes - print it
|
|
971 dcr_4:
|
|
972 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
973
|
|
974 movlw low( d'399' ) ; position of the label
|
|
975 movwf sub_a+0
|
|
976 movlw high( d'399' )
|
|
977 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
978 btfss print_compass_label ; Yes?
|
|
979 bra dcr_5
|
|
980 STRCPY_TEXT_PRINT tNE ; yes - print it
|
|
981 dcr_5:
|
|
982 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
983
|
|
984 movlw low( d'448' ) ; position of the label
|
|
985 movwf sub_a+0
|
|
986 movlw high( d'448' )
|
|
987 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
988 btfss print_compass_label ; Yes?
|
|
989 bra dcr_6
|
|
990 STRCPY_TEXT_PRINT tE ; yes - print it
|
|
991 dcr_6:
|
|
992 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
993
|
|
994 movlw low( d'489' ) ; position of the label
|
|
995 movwf sub_a+0
|
|
996 movlw high( d'489' )
|
|
997 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
998 btfss print_compass_label ; Yes?
|
|
999 bra dcr_7
|
|
1000 STRCPY_TEXT_PRINT tSE ; yes - print it
|
|
1001 dcr_7:
|
|
1002 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1003
|
|
1004 movlw low( d'538' ) ; position of the label
|
|
1005 movwf sub_a+0
|
|
1006 movlw high( d'538' )
|
|
1007 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
1008 btfss print_compass_label ; Yes?
|
|
1009 bra dcr_8
|
|
1010 STRCPY_TEXT_PRINT tS ; yes - print it
|
|
1011 dcr_8:
|
|
1012 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1013
|
|
1014 movlw low( d'579' ) ; position of the label
|
|
1015 movwf sub_a+0
|
|
1016 movlw high( d'579' )
|
|
1017 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
1018 btfss print_compass_label ; Yes?
|
|
1019 bra dcr_9
|
|
1020 STRCPY_TEXT_PRINT tSW ; yes - print it
|
|
1021 dcr_9:
|
|
1022 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1023
|
|
1024 movlw low( d'627' ) ; position of the label
|
|
1025 movwf sub_a+0
|
|
1026 movlw high( d'627' )
|
|
1027 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
1028 btfss print_compass_label ; Yes?
|
|
1029 bra dcr_10
|
|
1030 STRCPY_TEXT_PRINT tW ; yes - print it
|
|
1031 dcr_10:
|
|
1032 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1033
|
|
1034 movlw low( d'669' ) ; position of the label
|
|
1035 movwf sub_a+0
|
|
1036 movlw high( d'669' )
|
|
1037 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
1038 btfss print_compass_label ; Yes?
|
|
1039 bra dcr_11
|
|
1040 STRCPY_TEXT_PRINT tNW ; yes - print it
|
|
1041 dcr_11:
|
|
1042 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1043
|
|
1044 movlw low( d'718' ) ; position of the label
|
|
1045 movwf sub_a+0
|
|
1046 movlw high( d'718' )
|
|
1047 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
|
|
1048 btfss print_compass_label ; Yes?
|
|
1049 bra dcr_12
|
|
1050 STRCPY_TEXT_PRINT tN ; yes - print it
|
|
1051 dcr_12:
|
|
1052 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1053
|
|
1054 TFT_dive_compass_label_end:
|
|
1055 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1056 ; restore lo and hi for the final cleanup
|
|
1057 movff xLO,lo
|
|
1058 movff xHI,hi
|
|
1059 ; clear the rest of the SQ area if there are more space
|
370
|
1060 movlw d'159'
|
256
|
1061 cpfslt hi
|
|
1062 bra TFT_dive_compass_label_end2 ; D >= 160, no more space
|
|
1063 ; position left to end of display to clear the remaining area
|
370
|
1064 movlw d'158'
|
|
1065 movwf lo
|
256
|
1066 ; clear it
|
|
1067 rcall TFT_dive_compass_clr_label
|
|
1068 TFT_dive_compass_label_end2:
|
|
1069 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
|
|
1070 ; do we have bearing set?
|
|
1071 btfsc compass_bearing_set
|
|
1072 bra TFT_dive_compass_dir_text ; bearing_set=1 - go and print the dir (<< or >>)
|
|
1073 rcall TFT_dive_compass_dir_lclr ; no, clear the area (e.g. we had but removed)
|
|
1074 rcall TFT_dive_compass_dir_rclr
|
|
1075 bra TFT_dive_compass_text
|
|
1076
|
|
1077 TFT_dive_compass_dir_text:
|
|
1078 ; bearing set, but does it point to heading?
|
|
1079 btfss compass_bearing_eq
|
|
1080 bra TFT_dive_compass_dir_text_2 ; bearing != heading - go and print the dir
|
|
1081 rcall TFT_dive_compass_dir_lclr ; bearing = heading, no need for direction markers
|
|
1082 rcall TFT_dive_compass_dir_rclr
|
|
1083 bra TFT_dive_compass_text
|
|
1084
|
|
1085 TFT_dive_compass_dir_text_2:
|
257
|
1086 movlw color_green
|
|
1087 call TFT_set_color
|
256
|
1088 btfsc compass_bearing_lft
|
|
1089 bra TFT_dive_compass_dir_ldir ; bearing_lft=1, print the left marker
|
|
1090 ;TFT_dive_compass_text_rdir:
|
300
|
1091 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row
|
256
|
1092 STRCPY_PRINT ">>"
|
|
1093 ; do not forget to clear the left
|
|
1094 rcall TFT_dive_compass_dir_lclr
|
|
1095 bra TFT_dive_compass_text
|
|
1096
|
|
1097 TFT_dive_compass_dir_ldir:
|
300
|
1098 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row
|
256
|
1099 STRCPY_PRINT "<<"
|
|
1100 ; do not forget to clear the right
|
|
1101 rcall TFT_dive_compass_dir_rclr
|
|
1102 ;bra TFT_dive_compass_text
|
|
1103
|
|
1104 TFT_dive_compass_text:
|
371
|
1105 ; Clear some unused space on the right mH
|
|
1106 WIN_BOX_BLACK dm_custom_compass_tick_top_bot+.1,dm_custom_compass_tick_bot_top-.1,.158,.159 ; top, bottom, left, right
|
|
1107
|
256
|
1108 ; Text output
|
|
1109 call TFT_standard_color
|
300
|
1110 WIN_SMALL dm_custom_compass_head_column, dm_custom_compass_head_row
|
256
|
1111 rcall TFT_surface_compass_heading_com ; Show "000° N"
|
|
1112 return
|
|
1113
|
|
1114 TFT_dive_compass_dir_lclr:
|
300
|
1115 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row
|
256
|
1116 STRCPY_PRINT " "
|
|
1117 return
|
|
1118
|
|
1119 TFT_dive_compass_dir_rclr:
|
300
|
1120 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row
|
256
|
1121 STRCPY_PRINT " "
|
|
1122 return
|
|
1123
|
|
1124 TFT_dive_compass_label_proc:
|
386
|
1125 movwf sub_a+1
|
|
1126 movlw d'14'
|
|
1127 movwf up ; cardinal width in px
|
256
|
1128 bcf print_compass_label
|
|
1129 ; 1/a. check if it's viewable ? sub_a(RP) >= sub_b(RD) ?
|
|
1130 ; set the carry flag if sub_b(xRD) is equal to or greater than sub_a(xRP):
|
|
1131 movff xRD+0,sub_b+0
|
|
1132 movff xRD+1,sub_b+1
|
|
1133 call subU16 ; sub_c = sub_a - sub_b
|
|
1134 btfsc neg_flag ; >=0?
|
|
1135 return ; No
|
|
1136 ; store the RO=RP-RD for drawing
|
|
1137 movff sub_c+0,xC+0
|
|
1138 movff sub_c+1,xC+1
|
|
1139
|
|
1140 ; 1/b. check if it's viewable ? sub_a(RP)+up(width) < sub_b(RD)+160
|
|
1141 ; if already above, no need to process the rest of the labels
|
371
|
1142 movff up,WREG ; take care about the width
|
256
|
1143 addwf sub_a+0,1
|
|
1144 btfsc STATUS, C
|
|
1145 incf sub_a+1
|
|
1146
|
|
1147 movff xRDr+0,sub_b+0
|
|
1148 movff xRDr+1,sub_b+1
|
|
1149 call subU16 ; sub_c = sub_a - sub_b
|
|
1150 btfss neg_flag ; ? <0
|
|
1151 bra TFT_dive_compass_label_end ; No
|
|
1152
|
|
1153 ; 2. restore RO=RP-RD from 1/a.
|
|
1154 movff xC+0,lo
|
|
1155
|
|
1156 ; 3. Clear the segment from DD(hi) to lo
|
|
1157 ; don't do a clear if we are at 0 (zero) otherwise it will blink
|
|
1158 ; ?because of the width underflow?
|
370
|
1159 movlw d'1'
|
256
|
1160 cpfsgt lo
|
|
1161 bra TFT_dive_compass_label_proc_p
|
257
|
1162 rcall TFT_dive_compass_clr_label
|
256
|
1163 TFT_dive_compass_label_proc_p:
|
|
1164 ; 4. print the SQ on the screen
|
|
1165 call TFT_standard_color
|
|
1166 bsf print_compass_label
|
257
|
1167 ;TFT_dive_compass_label_print:
|
300
|
1168 movlw dm_custom_compass_label_row
|
257
|
1169 movff WREG,win_top
|
|
1170 movff lo,win_leftx2
|
|
1171 movlw FT_SMALL
|
|
1172 movff WREG,win_font
|
256
|
1173 ; 6. retain the new display positions
|
|
1174 movff lo,hi
|
|
1175 movff up,WREG
|
371
|
1176 addwf hi,F
|
256
|
1177 movff lo,xLO
|
|
1178 movff hi,xHI
|
|
1179 return
|
|
1180
|
|
1181 TFT_dive_compass_c_mk:
|
|
1182 ; Common task to draw center line and marker
|
|
1183 ; until a proper implementation make it simple:
|
|
1184 rcall TFT_dive_compass_mk
|
370
|
1185 movlw color_yellow
|
|
1186 WIN_BOX_COLOR dm_custom_compass_tick_top_top, dm_custom_compass_tick_bot_bot,.80,.81 ; center line in yellow
|
256
|
1187 return
|
|
1188
|
|
1189 TFT_dive_compass_mk:
|
|
1190 ; draw the bearing on the screen if visible and if we just put something over it
|
|
1191 btfss compass_bearing_set
|
|
1192 return ; bearing_set=0 nothing to display
|
|
1193
|
|
1194 btfss compass_bearing_vis
|
|
1195 return ; bearing set but not visible
|
|
1196
|
|
1197 ; save lo/hi from trashing
|
|
1198 movff lo,xA+0
|
|
1199 movff hi,xA+1
|
|
1200
|
|
1201 ; did we just update the marker's position?
|
|
1202 ; DD.......DD
|
|
1203 ; CM+2>=DD(old) or CM-2<=DD
|
|
1204 ; ToDo
|
|
1205
|
|
1206 btfss compass_bearing_ahd
|
|
1207 bra TFT_dive_compass_mk_rear
|
|
1208 ;TFT_dive_compass_mk_front:
|
|
1209 clrf lo
|
|
1210 movff xCM,lo
|
|
1211 bsf print_compass_label ; set=green marker
|
257
|
1212 rcall TFT_dive_compass_mk_print
|
256
|
1213 bcf print_compass_label
|
|
1214 bra TFT_dive_compass_mk_end
|
|
1215
|
|
1216 TFT_dive_compass_mk_rear:
|
|
1217 clrf lo
|
|
1218 movff xCM,lo
|
|
1219 bcf print_compass_label ; set=red marker
|
257
|
1220 rcall TFT_dive_compass_mk_print
|
256
|
1221
|
|
1222 TFT_dive_compass_mk_end:
|
|
1223 movff xA+0,lo
|
|
1224 movff xA+1,hi
|
|
1225 return
|
|
1226
|
|
1227 TFT_dive_compass_mk_print:
|
|
1228 movlw d'1'
|
|
1229 cpfsgt lo
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1230 bra TFT_dive_compass_mk_print_2 ; lo<=1, skip the first line
|
256
|
1231 movlw d'2'
|
|
1232 subwf lo,0
|
257
|
1233 ; movff WREG,win_leftx2
|
|
1234 rcall TFT_dive_compass_mk_print_3
|
256
|
1235 TFT_dive_compass_mk_print_2:
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1236 ; save hi/lo
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1237 movff hi,divA+1
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1238 movff lo,divA+0
|
371
|
1239 ; clear the middle of the bearing marker
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1240 movff lo,hi
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1241 movlw d'2'
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1242 addwf lo,1
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1243 rcall TFT_dive_compass_clr_label
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1244 ; restore hi/lo
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1245 movff divA+1,hi
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1246 movff divA+0,lo
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1247 ; print a dot on the middle
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1248 movff lo,WREG
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1249 rcall TFT_dive_compass_mk_print_dot
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1250 ; finally print the right marker line
|
256
|
1251 movlw d'2'
|
|
1252 addwf lo,0
|
257
|
1253 ; rcall TFT_dive_compass_mk_print_3
|
|
1254 ; return
|
|
1255 TFT_dive_compass_mk_print_3:
|
432
|
1256 movwf win_leftx2
|
300
|
1257 movlw dm_custom_compass_label_row
|
432
|
1258 movwf win_top
|
300
|
1259 movlw dm_custom_compass_label_height-.2
|
432
|
1260 movwf win_height
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1261 bra TFT_dive_compass_mk_print_4
|
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1262 TFT_dive_compass_mk_print_dot:
|
432
|
1263 movwf win_leftx2
|
300
|
1264 movlw dm_custom_compass_label_row + .9
|
432
|
1265 movwf win_top
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1266 movlw d'4'
|
432
|
1267 movwf win_height
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
diff
changeset
|
1268 TFT_dive_compass_mk_print_4:
|
371
|
1269 movlw .158
|
|
1270 cpfslt win_leftx2
|
|
1271 bra TFT_dive_compass_mk_print_5
|
256
|
1272 movlw d'2'
|
432
|
1273 movwf win_width+0
|
|
1274 clrf win_width+1
|
371
|
1275 movwf win_bargraph
|
256
|
1276 movlw color_green
|
|
1277 btfss print_compass_label
|
|
1278 movlw color_red
|
|
1279 call TFT_set_color
|
|
1280 call TFT_box
|
371
|
1281 TFT_dive_compass_mk_print_5:
|
|
1282 banksel common
|
256
|
1283 return
|
|
1284
|
|
1285 TFT_dive_compass_clr_label:
|
300
|
1286 movlw dm_custom_compass_label_row-.2 ; set top & height
|
256
|
1287 movff WREG,win_top
|
300
|
1288 movlw dm_custom_compass_label_height+.2
|
256
|
1289 movff WREG,win_height
|
257
|
1290 rcall TFT_dive_compass_clear
|
256
|
1291 return
|
|
1292
|
|
1293 TFT_dive_compass_clr_ruler:
|
|
1294 ; top tick
|
300
|
1295 movlw dm_custom_compass_tick_top_top ; set top & height
|
256
|
1296 movff WREG,win_top
|
300
|
1297 movlw dm_custom_compass_tick_height
|
256
|
1298 movff WREG,win_height
|
|
1299 rcall TFT_dive_compass_clear
|
|
1300 ;bottom tick
|
300
|
1301 movlw dm_custom_compass_tick_bot_top ; set top & height
|
256
|
1302 movff WREG,win_top
|
300
|
1303 movlw dm_custom_compass_tick_height
|
256
|
1304 movff WREG,win_height
|
257
|
1305 ; rcall TFT_dive_compass_clear
|
|
1306 ; return
|
256
|
1307 TFT_dive_compass_clear:
|
|
1308 ; we receive RM in lo and DD in hi
|
|
1309 ; calculate width = RM-D
|
|
1310 movff hi,WREG
|
371
|
1311 subwf lo,W
|
370
|
1312 bz TFT_dive_compass_clear3 ; Do nothing if there is nothing to do
|
432
|
1313 movwf win_width+0 ; RM-DD
|
|
1314 movwf win_bargraph
|
|
1315 clrf win_width+1
|
370
|
1316 movlw .1
|
432
|
1317 cpfsgt win_width+0
|
370
|
1318 bra TFT_dive_compass_clear3 ; Do not clear a single pixel (or less)
|
259
|
1319 movff hi,win_leftx2
|
256
|
1320 movlw color_black
|
|
1321 call TFT_set_color
|
|
1322 call TFT_box
|
370
|
1323 TFT_dive_compass_clear3:
|
256
|
1324 return
|
|
1325
|
|
1326 tft_compass_cardinal:
|
|
1327 btfsc hi,0 ; Heading >255°?
|
|
1328 bra tft_compass_cardinal2 ; Yes must be W, NW or N
|
|
1329 ; No, Must be W, SW, S, SE, E, NE or N
|
|
1330 movlw .23
|
|
1331 subwf lo,W
|
|
1332 btfss STATUS,C
|
|
1333 bra tft_compass_cardinal_N
|
|
1334 movlw .68
|
|
1335 subwf lo,W
|
|
1336 btfss STATUS,C
|
|
1337 bra tft_compass_cardinal_NE
|
|
1338 movlw .113
|
|
1339 subwf lo,W
|
|
1340 btfss STATUS,C
|
|
1341 bra tft_compass_cardinal_E
|
|
1342 movlw .158
|
|
1343 subwf lo,W
|
|
1344 btfss STATUS,C
|
|
1345 bra tft_compass_cardinal_SE
|
|
1346 movlw .203
|
|
1347 subwf lo,W
|
|
1348 btfss STATUS,C
|
|
1349 bra tft_compass_cardinal_S
|
|
1350 movlw .248
|
|
1351 subwf lo,W
|
|
1352 btfss STATUS,C
|
|
1353 bra tft_compass_cardinal_SW
|
|
1354 bra tft_compass_cardinal_W
|
|
1355
|
|
1356 tft_compass_cardinal2:
|
|
1357 movlw .37
|
|
1358 subwf lo,W
|
|
1359 btfss STATUS,C
|
|
1360 bra tft_compass_cardinal_W
|
|
1361 movlw .82
|
|
1362 subwf lo,W
|
|
1363 btfss STATUS,C
|
|
1364 bra tft_compass_cardinal_NW
|
|
1365 ; bra tft_compass_cardinal_N
|
|
1366 tft_compass_cardinal_N:
|
|
1367 STRCAT_TEXT tN
|
|
1368 return
|
|
1369 tft_compass_cardinal_NE:
|
|
1370 STRCAT_TEXT tNE
|
|
1371 return
|
|
1372 tft_compass_cardinal_E:
|
|
1373 STRCAT_TEXT tE
|
|
1374 return
|
|
1375 tft_compass_cardinal_SE:
|
|
1376 STRCAT_TEXT tSE
|
|
1377 return
|
|
1378 tft_compass_cardinal_S:
|
|
1379 STRCAT_TEXT tS
|
|
1380 return
|
|
1381 tft_compass_cardinal_SW:
|
|
1382 STRCAT_TEXT tSW
|
|
1383 return
|
|
1384 tft_compass_cardinal_W:
|
|
1385 STRCAT_TEXT tW
|
|
1386 return
|
|
1387 tft_compass_cardinal_NW:
|
|
1388 STRCAT_TEXT tNW
|
|
1389 return
|
|
1390
|
|
1391 compass_heading_common:
|
|
1392 call speed_normal
|
|
1393 movlw compass_averaging ; numbers of extra averaging
|
|
1394 movwf up
|
|
1395 compass_heading_common2:
|
|
1396 rcall TFT_get_compass
|
|
1397 decfsz up,F
|
|
1398 bra compass_heading_common2
|
|
1399 extern compass
|
|
1400 call compass ; Do compass corrections.
|
|
1401 banksel common
|
|
1402
|
|
1403 ; More then compass_fast_treshold?
|
|
1404 movff compass_heading_old+0,sub_a+0
|
|
1405 movff compass_heading_old+1,sub_a+1
|
|
1406 movff compass_heading+0,sub_b+0
|
|
1407 movff compass_heading+1,sub_b+1
|
|
1408 call sub16
|
|
1409 btfss neg_flag ; <0?
|
|
1410 bra compass_heading_common3 ; No, test for threshold
|
|
1411 ; Yes, subtract the other way round
|
|
1412 movff compass_heading+0,sub_a+0
|
|
1413 movff compass_heading+1,sub_a+1
|
|
1414 movff compass_heading_old+0,sub_b+0
|
|
1415 movff compass_heading_old+1,sub_b+1
|
|
1416 call sub16
|
|
1417 compass_heading_common3:
|
|
1418 movff compass_heading+0,compass_heading_old+0 ; copy new "old"
|
|
1419 movff compass_heading+1,compass_heading_old+1
|
|
1420
|
|
1421 bcf compass_fast_mode
|
|
1422 movlw compass_fast_treshold
|
|
1423 cpfslt sub_c+0 ; > compass_fast_treshold?
|
|
1424 bsf compass_fast_mode ; Yes!
|
|
1425
|
|
1426 btfss compass_fast_mode ; In fast mode?
|
|
1427 return ; No.
|
|
1428 ; Yes.
|
|
1429 movff compass_heading+0,lo
|
|
1430 movff compass_heading+1,hi
|
|
1431 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
|
|
1432 movff lo,compass_heading_shown+0
|
|
1433 movff hi,compass_heading_shown+1
|
|
1434 return
|
|
1435
|
|
1436 TFT_get_compass:
|
|
1437 call I2C_RX_compass ; Test Compass
|
|
1438 call I2C_RX_accelerometer ; Test Accelerometer
|
|
1439 call compass_filter ; Filter Raw compass + accel readings.
|
|
1440 banksel common
|
|
1441 return
|
|
1442
|
382
|
1443 global TFT_surf_set_bearing
|
|
1444 TFT_surf_set_bearing:
|
|
1445 btfsc premenu
|
|
1446 return ; Already shown, return
|
|
1447 bsf premenu ; set flag
|
|
1448 WIN_BOX_BLACK surf_compass_bear_row,surf_warning1_row-1, surf_compass_bear_column, surf_decotype_column-.1 ; top, bottom, left, right
|
|
1449 WIN_SMALL surf_compass_bear_column,surf_compass_bear_row
|
|
1450 WIN_COLOR color_yellow
|
|
1451 bsf win_invert
|
|
1452 STRCPY_TEXT_PRINT tSetHeading ; 7 chars
|
|
1453 bcf win_invert
|
|
1454 return
|
147
|
1455
|
0
|
1456 END
|