Mercurial > public > hwos_code

comparison src/compass_ops.asm @ 623:c40025d8e750

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