Mercurial > public > hwos_code

comparison src/compass_ops.asm @ 628:cd58f7fc86db

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
3.05 stable work
author heinrichsweikamp
date Thu, 19 Sep 2019 12:01:29 +0200
parents c40025d8e750
children 237931377539
comparison
equal deleted inserted replaced
627:bf5fee575701 628:cd58f7fc86db
1 ;============================================================================= 1 ;=============================================================================
2 ; 2 ;
3 ; File compass_ops.asm combined next generation V3.03.2 3 ; File compass_ops.asm combined next generation V3.03.5
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 ;=============================================================================
168 movf PRODL,W 168 movf PRODL,W
169 addwf compass_r+2,F ; fraction part to carry 169 addwf compass_r+2,F ; fraction part to carry
170 movf PRODH,W ; and add16 170 movf PRODH,W ; and add16
171 addwfc compass_r+0,F 171 addwfc compass_r+0,F
172 movlw 0 172 movlw 0
173 addwfc compass_r+1,F 173 addwfc compass_r+1,F
174 174
175 movf compass_a+0,W ; block al*bh 175 movf compass_a+0,W ; block al*bh
176 mulwf compass_b+1 176 mulwf compass_b+1
177 movf PRODL,W 177 movf PRODL,W
178 addwf compass_r+2,F ; fraction part to carry 178 addwf compass_r+2,F ; fraction part to carry
207 subwf compass_r+0,F 207 subwf compass_r+0,F
208 movf compass_b+1,W 208 movf compass_b+1,W
209 subwfb compass_r+1,F 209 subwfb compass_r+1,F
210 210
211 compass_mul_4: 211 compass_mul_4:
212 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
213 btfsc compass_r+1,7 213 btfsc compass_r+1,7
214 bsf compass_r+1,6 214 bsf compass_r+1,6
215 bra compass_mul_2 215 bra compass_mul_2
216 216
217 ;-----------------------------------------------------------------------------
217 218
218 global compass_calibration_loop 219 global compass_calibration_loop
219 compass_calibration_loop: ; compass calibration 220 compass_calibration_loop: ; compass calibration
220 bsf block_sensor_interrupt ; disable sensor interrupts 221 bsf block_sensor_interrupt ; disable sensor interrupts
221 call I2C_sleep_accelerometer ; stop accelerometer 222 call I2C_sleep_compass ; stop compass
222 call I2C_sleep_compass ; stop compass
223 call TFT_ClearScreen 223 call TFT_ClearScreen
224 ; Mask 224 ; Mask
225 WIN_COLOR color_greenish 225 WIN_COLOR color_greenish
226 WIN_SMALL .16,.0 226 WIN_SMALL .16,.0
227 STRCPY_TEXT_PRINT tCompassMenu 227 STRCPY_TEXT_PRINT tCompassMenu
228 btfss switch_right2 ; wait until button is released 228 btfss switch_right2 ; wait until button is released
229 bra $-2 229 bra $-2
230 230
231 call TFT_standard_color 231 call TFT_standard_color
232 ; WIN_SMALL .0,.215 232 ; WIN_SMALL .0,.215
233 ; STRCPY_TEXT_PRINT tExit 233 ; STRCPY_TEXT_PRINT tExit
234 WAITMS d'255' 234 WAITMS d'255'
235 WAITMS d'255' 235 WAITMS d'255'
236 236
237 call request_speed_fastest ; request CPU speed change to fastest speed 237 call request_speed_fastest ; request CPU speed change to fastest speed
238 238
239 movlw .7 ; initialize gain 239 movlw .7 ; initialize gain
240 movff WREG,opt_compass_gain 240 movff WREG,opt_compass_gain
241 241
242 movlw .60 ; initialize timeout to 60 seconds 242 movlw .60 ; initialize timeout to 60 seconds
243 movwf isr_timeout_reload ; copy WREG to isr_timeout_reload 243 movwf isr_timeout_reload ; copy WREG to isr_timeout_reload
244 bsf reset_timeout ; request ISR to reset the timeout 244 bsf reset_timeout ; request ISR to reset the timeout
245 bcf trigger_timeout ; clear any pending timeout trigger 245 bcf trigger_timeout ; clear any pending timeout trigger
246 compass_calibration_gainset: ; reduce the gain, set bank here! 246 compass_calibration_gainset: ; reduce the gain, set bank here!
247 banksel opt_compass_gain ; select bank options table 247 banksel opt_compass_gain ; select bank options table
248 decf opt_compass_gain,F ; reduce by one 248 decf opt_compass_gain,F ; reduce by one
249 btfsc STATUS,N ; < 0 ? 249 btfsc STATUS,N ; < 0 ?
250 clrf opt_compass_gain ; YES - keep at zero 250 clrf opt_compass_gain ; YES - keep at zero
251 banksel common ; bank to bank common 251 btfsc STATUS,N ; < 0 ?
252 252 bra compass_calibration_loop1 ; YES - skip gain stuff (Would hang here in case of compass failure)
253 call I2C_init_accelerometer 253 banksel common ; bank to bank common
254
254 call I2C_init_compass 255 call I2C_init_compass
255 256
256 ; btfsc compass_type ; compass1 ? 257 btfsc compass_type3 ; compass type 3 ?
257 ; bra compass_calibration_loop1 ; YES - skip gain stuff 258 bra compass_calibration_loop1 ; YES - skip gain stuff
258 259 btfsc compass_type2 ; compass type 2 ?
259 rcall TFT_compass_show_gain ; show the current compass gain 260 bra compass_calibration_loop1 ; YES - skip gain stuff
261
262 rcall TFT_compass_show_gain ; show the current compass gain
260 263
261 WAITMS d'250' 264 WAITMS d'250'
262 WAITMS d'250' ; wait for first reading... 265 WAITMS d'250' ; wait for first reading...
263 266
264 movlw .60 ; calibration shall run for 60 seconds 267 movlw .60 ; calibration shall run for 60 seconds
265 call reset_timeout_time ; set timeout 268 call reset_timeout_time ; set timeout
266 269
267 call I2C_RX_compass ; read compass 270 call I2C_RX_compass ; read compass
268 call I2C_RX_accelerometer ; read accelerometer 271 call I2C_RX_accelerometer ; read accelerometer
269 272
270 ; Test all axes for +4096 (Hi byte=16) 273 ; Test all axes for +4096 (Hi byte=16)
271 banksel compass_DX ; select bank common2 274 banksel compass_DX ; select bank common2
272 movlw .16 275 movlw .16
273 cpfseq compass_DX+1 276 cpfseq compass_DX+1
274 bra $+4 277 bra $+4
275 bra compass_calibration_gainset 278 bra compass_calibration_gainset
276 cpfseq compass_DY+1 279 cpfseq compass_DY+1
289 bra $+4 292 bra $+4
290 bra compass_calibration_gainset 293 bra compass_calibration_gainset
291 cpfseq compass_DZ+1 294 cpfseq compass_DZ+1
292 bra $+4 295 bra $+4
293 bra compass_calibration_gainset 296 bra compass_calibration_gainset
294 banksel common ; back to bank common 297
295 298 compass_calibration_loop1: ; done with gain
296 compass_calibration_loop1: ; done with gain 299 banksel common ; bank to bank common
297 rcall compass_filter_init ; set DX_f values 300 rcall compass_filter_init ; set DX_f values
298 call compass_reset_calibration ; reset CX_f values (C-code) 301 call compass_reset_calibration ; reset CX_f values (C-code)
299 banksel common ; back to bank common 302 banksel common ; back to bank common
300 303
301 compass_calibration_loop2: 304 compass_calibration_loop2:
302 call I2C_RX_compass ; read compass 305 call I2C_RX_compass ; read compass
303 call I2C_RX_accelerometer ; test accelerometer 306 call I2C_RX_accelerometer ; test accelerometer
304 rcall compass_filter ; filter compass raw data 307 rcall compass_filter ; filter compass raw data
305 308
306 ; Twice 309 ; Twice
307 call I2C_RX_compass ; read compass 310 call I2C_RX_compass ; read compass
308 call I2C_RX_accelerometer ; test accelerometer 311 call I2C_RX_accelerometer ; test accelerometer
309 rcall compass_filter ; filter compass raw data 312 rcall compass_filter ; filter compass raw data
310 313
311 ; btfsc compass_type ; compass1? 314 ; btfsc compass_type1 ; compass1?
312 ; bra compass_calibration_loop3 ; YES - skip gain stuff 315 ; bra compass_calibration_loop3 ; YES - skip gain stuff
313 316
314 ; Test all axes for +4096 (Hi byte=16) 317 ; Test all axes for +4096 (Hi byte=16)
315 banksel compass_DX ; select bank common2 318 banksel compass_DX ; select bank common2
316 movlw .16 319 movlw .16
317 cpfseq compass_DX+1 320 cpfseq compass_DX+1
318 bra $+4 321 bra $+4
319 bra compass_calibration_gainset 322 bra compass_calibration_gainset
320 cpfseq compass_DY+1 323 cpfseq compass_DY+1
333 bra $+4 336 bra $+4
334 bra compass_calibration_gainset 337 bra compass_calibration_gainset
335 cpfseq compass_DZ+1 338 cpfseq compass_DZ+1
336 bra $+4 339 bra $+4
337 bra compass_calibration_gainset 340 bra compass_calibration_gainset
338 banksel common ; back to bank common 341 banksel common ; back to bank common
339 ; 342 ;
340 ; ; Three 343 ; ; Three
341 ; call I2C_RX_compass ; read compass 344 ; call I2C_RX_compass ; read compass
342 ; call I2C_RX_accelerometer ; test accelerometer 345 ; call I2C_RX_accelerometer ; test accelerometer
343 ; call compass_filter ; filter compass raw data 346 ; call compass_filter ; filter compass raw data
344 ; 347 ;
345 ; ; Four times to get cleaner values 348 ; ; Four times to get cleaner values
346 ; call I2C_RX_compass ; read compass 349 ; call I2C_RX_compass ; read compass
347 ; call I2C_RX_accelerometer ; test accelerometer 350 ; call I2C_RX_accelerometer ; test accelerometer
348 ; call compass_filter ; filter compass raw data 351 ; call compass_filter ; filter compass raw data
349 352
350 compass_calibration_loop3: 353 compass_calibration_loop3:
351 ; and register only one value out of four: 354 ; and register only one value out of four:
352 call compass_add_calibration ; check and store new max/min values (C-code) 355 call compass_add_calibration ; check and store new max/min values (C-code)
353 banksel common ; back to bank common 356 banksel common ; back to bank common
354 357
355 rcall TFT_compass_fast ; show values 358 rcall TFT_compass_fast ; show values
356 btfsc trigger_timeout ; timeout (calibration done)? 359 btfsc trigger_timeout ; timeout (calibration done)?
357 bra compass_calibration_exit ; YES - exit 360 bra compass_calibration_exit ; YES - done
358 btfss trigger_full_second ; NO - new second begun? 361 btfss trigger_full_second ; NO - new second begun?
359 bra compass_calibration_loop2 ; NO - loop 362 bra compass_calibration_loop2 ; NO - loop
360 bcf trigger_full_second ; YES - clear flag 363 bcf trigger_full_second ; YES - clear flag
361 rcall TFT_show_timeout_testmode ; - show remaining time 364 rcall TFT_show_timeout_testmode ; - show remaining time
362 bra compass_calibration_loop2 ; - loop 365 bra compass_calibration_loop2 ; - loop
363 366
364 compass_calibration_exit: 367 compass_calibration_exit:
365 bcf block_sensor_interrupt ; re-enable sensor interrupts 368 bcf block_sensor_interrupt ; re-enable sensor interrupts
366 369
367 call compass_solve_calibration ; calculate calibration factors (C-code) 370 call compass_solve_calibration ; calculate calibration factors (C-code)
368 banksel common ; back to bank common 371 banksel common ; back to bank common
369 372
370 call request_speed_normal ; request CPU speed change to normal speed 373 call request_speed_normal ; request CPU speed change to normal speed
371 374
372 call option_save_all ; save all settings into EEPROM 375 call option_save_all ; save all settings into EEPROM
373 movlw .6 376 movlw .6
374 movff WREG,customview_surfmode ; set to compass view... 377 movff WREG,customview_surfmode ; set to compass view...
375 goto surfloop ; ...and exit 378 goto surfloop ; ...and exit
376 379
380 ;-----------------------------------------------------------------------------
377 381
378 global TFT_compass_fast 382 global TFT_compass_fast
379 TFT_compass_fast: 383 TFT_compass_fast:
380 WIN_TINY .20,.50 384 WIN_TINY .0,.50
381 STRCPY "X:" 385 STRCPY "Cx:"
382 MOVII compass_DX,mpr 386 MOVII compass_DX,mpr
383 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required 387 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required
384 output_16 388 output_16
385 STRCAT " Y:" 389 STRCAT " Cy:"
386 MOVII compass_DY,mpr 390 MOVII compass_DY,mpr
387 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required 391 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required
388 output_16 392 output_16
389 STRCAT " Z:" 393 STRCAT " Cz:"
390 MOVII compass_DZ,mpr 394 MOVII compass_DZ,mpr
391 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required 395 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required
392 output_16 396 output_16
393 STRCAT_PRINT " " 397 STRCAT_PRINT " "
398
399 WIN_TINY .0,.104
400 STRCPY "Ax:"
401 MOVII accel_DX,mpr
402 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required
403 output_16
404 STRCAT " Ay:"
405 MOVII accel_DY,mpr
406 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required
407 output_16
408 STRCAT " Az:"
409 MOVII accel_DZ,mpr
410 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required
411 output_16
412 STRCAT_PRINT " "
394 return 413 return
395 414
396 TFT_show_timeout_testmode: 415 TFT_show_timeout_testmode:
397 WIN_TINY .20,.68 416 WIN_TINY .0,.68
398 STRCPY "T:" 417 STRCPY "T:"
399 movff isr_timeout_timer,lo 418 movff isr_timeout_timer,lo
400 bsf leftbind 419 bsf leftbind
401 output_8 ; display remaining time 420 output_8 ; display remaining time
402 bcf leftbind 421 bcf leftbind
403 STRCAT_PRINT "s " 422 STRCAT_PRINT "s "
404 return 423 return
405 424
406 TFT_compass_show_gain: ; show the current compass gain 425 TFT_compass_show_gain: ; show the current compass gain
407 ; movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) 426 ; movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss)
408 ; tstfsz lo 427 ; tstfsz lo
409 ; return ; do not show unless gain=0 428 ; return ; do not show unless gain=0
410 WIN_TINY .20,.86 429 WIN_TINY .0,.86
411 STRCPY_TEXT tCompassGain 430 STRCPY_TEXT tCompassGain
412 movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) 431 movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss)
413 bsf leftbind 432 bsf leftbind
414 output_8 433 output_8
415 bcf leftbind 434 bcf leftbind
416 STRCAT_PRINT "" 435 STRCAT_PRINT ""
417 return 436 return
418 437
419 438 ;-----------------------------------------------------------------------------
420 TFT_surface_compass_bearing:
421 WIN_SMALL surf_compass_bear_column,surf_compass_bear_row
422 MOVII compass_bearing,mpr
423 PUTC "("
424 bsf leftbind
425 output_16dp .2 ; result is "0.000"
426 bcf leftbind
427 ; rearrange figures to "000"
428 movff buffer+3,buffer+1
429 movff buffer+4,buffer+2
430 movff buffer+5,buffer+3
431 lfsr FSR2,buffer+4
432 STRCAT "° "
433 rcall tft_compass_cardinal ; add cardinal and ordinal to POSTINC2
434 STRCAT_PRINT ")"
435 return
436
437 439
438 global TFT_surface_compass_mask 440 global TFT_surface_compass_mask
439 TFT_surface_compass_mask: 441 TFT_surface_compass_mask:
440 WIN_SMALL surf_compass_mask_column,surf_compass_mask_row 442 WIN_SMALL surf_compass_mask_column,surf_compass_mask_row
441 call TFT_standard_color 443 call TFT_standard_color
442 STRCPY_TEXT_PRINT tHeading ; Heading: 444 STRCPY_TEXT_PRINT tHeading ; print "Heading:"
443 return 445 return
444 446
445 447
446 global TFT_dive_compass_mask ; draws the white box around the heading tape 448 global TFT_dive_compass_mask ; draws the white box around the heading tape
447 TFT_dive_compass_mask: 449 TFT_dive_compass_mask:
448 WIN_FRAME_STD dm_custom_compass_graph_row, dm_custom_compass_graph_row+dm_custom_compass_graph_height, .0, .159 450 WIN_FRAME_STD dm_custom_compass_graph_row, dm_custom_compass_graph_row+dm_custom_compass_graph_height, .0, .159
449 return 451 return
450 452
451 453
452 global TFT_surface_compass_heading 454 global TFT_surface_compass_heading
453 TFT_surface_compass_heading: 455 TFT_surface_compass_heading:
454 rcall compass_heading_common 456 rcall compass_heading_common
455 WIN_STD surf_compass_head_column,surf_compass_head_row 457 WIN_STD surf_compass_head_column,surf_compass_head_row
456 call TFT_standard_color 458 call TFT_standard_color
457 TFT_surface_compass_heading_com: ; show "000° N" 459 TFT_surface_compass_heading_com: ; show "000° N"
458 MOVII compass_heading_new,mpr 460 movff compass_heading_new+1,WREG ; get upper byte of actual heading
459 btfss mpr+1,7 ; compass calibrated? 461 btfsc WREG,7 ; compass calibrated?
460 bra TFT_surface_compass_heading_com0 ; YES 462 bra TFT_compass_uncalibrated ; NO
461 STRCAT_PRINT "---°" ; NO - print "---°" 463 MOVII compass_heading_shown,mpr ; get heading to be shown
462 return ; - done 464 rcall TFT_compass_helper ; show heading and its cardinal
463 TFT_surface_compass_heading_com0: 465 btfsc divemode ; in dive mode?
464 ; Shown and actual identical? 466 return ; YES - done for dive mode
465 movff compass_heading_shown+0,WREG ; get heading shown, low byte 467 ; in surface mode - shall show bearing?
466 cpfseq mpr+0 ; compare with current heading, equal? 468 btfss compass_bearing_set ; is a bearing set?
467 bra TFT_surface_compass_heading_com1 ; NO - not equal 469 return ; NO - done
468 movff compass_heading_shown+1,WREG ; get heading shown, high byte 470 btfsc compass_menu ; is the "set bearing" selection shown?
469 cpfseq mpr+1 ; compare with current heading, equal? 471 return ; YES - done
470 bra TFT_surface_compass_heading_com1 ; NO - not equal 472 ; show bearing
471 bra TFT_surface_compass_heading_com3 ; YES - skip smoothing 473 WIN_SMALL surf_compass_bear_column,surf_compass_bear_row
472 474 call TFT_attention_color
473 TFT_surface_compass_heading_com1: 475 MOVII compass_bearing,mpr ; get bearing
474 MOVII mpr,sub_a 476 ;bra TFT_compass_helper ; show number and cardinal and return
475 MOVII compass_heading_shown,sub_b 477
476 call subU16 478 TFT_compass_helper:
477 btfsc neg_flag
478 bra TFT_surface_compass_heading_com2 ; shown > actual
479 ; shown < actual
480 banksel compass_heading_shown ; select bank common2
481 INCI compass_heading_shown ; compass_heading_shown++
482 banksel common ; back to bank common
483 bra TFT_surface_compass_heading_com3
484
485 TFT_surface_compass_heading_com2:
486 banksel compass_heading_shown ; select bank common2
487 DECI compass_heading_shown ; compass_heading_shown--
488 banksel common ; back to bank common
489
490 TFT_surface_compass_heading_com3:
491 MOVII compass_heading_shown,mpr
492 bsf leftbind 479 bsf leftbind
493 output_16dp .2 ; result is "0.000" 480 output_16dp .2 ; result is "0.000" in buffer
494 bcf leftbind 481 bcf leftbind
495 ; rearrange figures to "000" 482 ; rearrange figures to "000"
496 movff buffer+2,buffer+0 483 movff buffer+2,buffer+0
497 movff buffer+3,buffer+1 484 movff buffer+3,buffer+1
498 movff buffer+4,buffer+2 485 movff buffer+4,buffer+2
499 lfsr FSR2,buffer+3 486 lfsr FSR2,buffer+3
500 STRCAT "° " 487 STRCAT "° "
501 rcall tft_compass_cardinal ; add cardinal and ordinal to POSTINC2 488 rcall tft_compass_cardinal ; add cardinal to POSTINC2
502 clrf WREG 489 STRCAT_PRINT "" ; finalize output
503 movff WREG,buffer+.7 ; limit to 7 chars 490 return ; done
504 STRCAT_PRINT "" 491
505 btfsc divemode ; in dive mode? 492 TFT_compass_uncalibrated:
506 return ; YES - done for dive mode 493 STRCAT_PRINT "---°" ; print "---°"
507 ; show bearing on the surface? 494 return ; done
508 btfss compass_bearing_set ; is a bearing set?
509 return ; NO - return
510 btfsc compass_menu ; is the "set bearing" selection shown?
511 return ; YES - return
512 bra TFT_surface_compass_bearing ; NO - show bearing and return
513 495
514 496
515 global TFT_dive_compass_heading 497 global TFT_dive_compass_heading
516 TFT_dive_compass_heading: 498 TFT_dive_compass_heading:
517 WIN_FONT FT_SMALL ; set font size TODO - needed ???
518 rcall compass_heading_common 499 rcall compass_heading_common
500 WIN_FONT FT_SMALL ; set font size
501
519 ; ; ### for development only, hard-coding the bearing ### 502 ; ; ### for development only, hard-coding the bearing ###
520 ; ; 244° : SW - W 503 ; ; 244° : SW - W
521 ; MOVLI .244,xA ; xA used as temp 504 ; MOVLI .244,xA ; xA used as temp
522 ; MOVII xA,compass_bearing ; compass_bearing is stored in bank isr_backup 505 ; MOVII xA,compass_bearing ; compass_bearing is stored in bank isr_backup
523 506
524 MOVII compass_heading_shown,xA 507 MOVII compass_heading_shown,xA
525 ; 160° viewing angle: add +360 offset if xA <= 292 for non-negative scale 508 ; 160° viewing angle: add +360 offset if xA <= 292 for non-negative scale
526 MOVLI .292,sub_a 509 MOVLI .292,sub_a
527 MOVII xA, sub_b 510 MOVII xA, sub_b
528 call subU16 ; sub_c = sub_a - sub_b 511 call subU16 ; sub_c = sub_a - sub_b
529 btfsc neg_flag ; xA > 292 ? 512 btfsc neg_flag ; xA > 292 ?
530 bra TFT_dive_compass_heading_1 ; YES 513 bra TFT_dive_compass_heading_1 ; YES
531 ADDLI .360,xA ; NO - add offset 514 ADDLI .360,xA ; NO - add offset
532 TFT_dive_compass_heading_1: 515 TFT_dive_compass_heading_1:
533 SUBLI .80,xA ; subtract 80 (left pixel offset from the center) 516 SUBLI .80,xA ; subtract 80 (left pixel offset from the center)
534 MOVII xA,xRD ; save result to xRD 517 MOVII xA,xRD ; save result to xRD
535 ADDLI .160,xA ; add 160 (display with in pixels) 518 ADDLI .160,xA ; add 160 (display with in pixels)
536 MOVII xA,xRDr ; save result to xRDr 519 MOVII xA,xRDr ; save result to xRDr
537 520
538 btfss compass_bearing_set ; is a bearing set? 521 btfss compass_bearing_set ; is a bearing set?
539 bra TFT_dive_compass_ruler ; NO - skip next calculations 522 bra TFT_dive_compass_ruler ; NO - skip next calculations
540 MOVII xRDr,sub_a ; YES - calculate xRD180 = xRDr - 180 523 MOVII xRDr,sub_a ; YES - calculate xRD180 = xRDr - 180
541 MOVLI .180,sub_b 524 MOVLI .180,sub_b
542 call subU16 ; sub_c = sub_a - sub_b 525 call subU16 ; sub_c = sub_a - sub_b
543 MOVII sub_c,xRD180 526 MOVII sub_c,xRD180
544 527
545 TFT_dive_compass_bearing_1: 528 TFT_dive_compass_bearing_1:
546 ; calculate bearing position and visibility (ahead or behind) 529 ; calculate bearing position and visibility (ahead or behind)
547 bcf compass_bearing_vis ; default is not-visible 530 bcf compass_bearing_vis ; default is not-visible
548 bcf compass_bearing_ahd ; default is behind 531 bcf compass_bearing_ahd ; default is behind
549 MOVII compass_bearing,xA 532 MOVII compass_bearing,xA
550 ADDLI .360,xA ; calculate the bearing virtual display offset 533 ADDLI .360,xA ; calculate the bearing virtual display offset
551 MOVII xA,divA ; save it for reuse for upper/lower turns and ahead/behind checks 534 MOVII xA,divA ; save it for reuse for upper/lower turns and ahead/behind checks
552 535
553 ; check if bearing is ahead 536 ; check if bearing is ahead
554 MOVII divA,sub_a ; load the bearing offset into sub_a 537 MOVII divA,sub_a ; load the bearing offset into sub_a
555 MOVII xRD, sub_b ; load the display offset back to sub_b 538 MOVII xRD, sub_b ; load the display offset back to sub_b
556 rcall TFT_dive_compass_bearing_ap 539 rcall TFT_dive_compass_bearing_ap
557 btfsc compass_bearing_vis ; bearing visible? 540 btfsc compass_bearing_vis ; bearing visible?
558 bra TFT_dive_compass_bearing_dir ; YES 541 bra TFT_dive_compass_bearing_dir; YES
559 542
560 ; check if it's ahead with an upper turn 543 ; check if it is ahead with an upper turn
561 MOVII divA,sub_a ; load the bearing offset into sub_a 544 MOVII divA,sub_a ; load the bearing offset into sub_a
562 MOVII xRD, sub_b ; load the display offset back to sub_b 545 MOVII xRD, sub_b ; load the display offset back to sub_b
563 ADDLI .360,sub_b 546 ADDLI .360,sub_b
564 rcall TFT_dive_compass_bearing_ap 547 rcall TFT_dive_compass_bearing_ap
565 btfsc compass_bearing_vis ; bearing visible? 548 btfsc compass_bearing_vis ; bearing visible?
566 bra TFT_dive_compass_bearing_dir ; YES 549 bra TFT_dive_compass_bearing_dir; YES
567 550
568 ; check if it's ahead with a lower turn 551 ; check if it is ahead with a lower turn
569 MOVII divA,sub_a ; load the bearing offset into sub_a 552 MOVII divA,sub_a ; load the bearing offset into sub_a
570 ADDLI .360,sub_a 553 ADDLI .360,sub_a
571 MOVII xRD, sub_b ; load the display offset back to sub_b 554 MOVII xRD, sub_b ; load the display offset back to sub_b
572 rcall TFT_dive_compass_bearing_ap 555 rcall TFT_dive_compass_bearing_ap
573 btfsc compass_bearing_vis ; bearing visible? 556 btfsc compass_bearing_vis ; bearing visible?
574 bra TFT_dive_compass_bearing_dir ; YES 557 bra TFT_dive_compass_bearing_dir; YES
575 558
576 ; marker is not ahead of us, check if it is behind of us 559 ; marker is not ahead of us, check if it is behind of us
577 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display 560 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display
578 MOVII xRD180,sub_a ; load the display offset back to sub_a 561 MOVII xRD180,sub_a ; load the display offset back to sub_a
579 MOVII divA, sub_b ; load the marker's offset into sub_b 562 MOVII divA, sub_b ; load the marker's offset into sub_b
580 rcall TFT_dive_compass_bearing_bp 563 rcall TFT_dive_compass_bearing_bp
581 btfsc compass_bearing_vis ; bearing behind of us? 564 btfsc compass_bearing_vis ; bearing behind of us?
582 bra TFT_dive_compass_bearing_dir ; YES 565 bra TFT_dive_compass_bearing_dir; YES
583 566
584 ; check if it's behind with the upper turn 567 ; check if it is behind with the upper turn
585 MOVII xRD180,sub_a 568 MOVII xRD180,sub_a
586 MOVII divA, sub_b 569 MOVII divA, sub_b
587 ADDLI .360, sub_b 570 ADDLI .360, sub_b
588 rcall TFT_dive_compass_bearing_bp 571 rcall TFT_dive_compass_bearing_bp
589 btfsc compass_bearing_vis ; bearing behind of us? 572 btfsc compass_bearing_vis ; bearing behind of us?
590 bra TFT_dive_compass_bearing_dir ; YES 573 bra TFT_dive_compass_bearing_dir; YES
591 574
592 ; check if it's behind with the lower turn 575 ; check if it is behind with the lower turn
593 MOVII xRD180,sub_a 576 MOVII xRD180,sub_a
594 ADDLI .360, sub_a 577 ADDLI .360, sub_a
595 MOVII divA, sub_b ; load the marker's offset into sub_b 578 MOVII divA, sub_b ; load the marker's offset into sub_b
596 rcall TFT_dive_compass_bearing_bp 579 rcall TFT_dive_compass_bearing_bp
597 bra TFT_dive_compass_bearing_dir 580 bra TFT_dive_compass_bearing_dir
598 581
599 TFT_dive_compass_bearing_ap: 582 TFT_dive_compass_bearing_ap:
600 ; xCM received in sub_a 583 ; xCM received in sub_a
601 ; xRD received in sub_b 584 ; xRD received in sub_b
602 ; 1/a. check if it's viewable from the left side 585 ; 1/a. check if it's viewable from the left side
603 call subU16 ; sub_c = sub_a - sub_b 586 call subU16 ; sub_c = sub_a - sub_b
604 btfsc neg_flag ; xRD > divA ? 587 btfsc neg_flag ; xRD > divA ?
605 return ; NO - done 588 return ; NO - done
606 MOVII sub_c,xC ; YES - store the RO=RP-RD for drawing 589 MOVII sub_c,xC ; YES - store the RO=RP-RD for drawing
607 ; 1/b. check if it's viewable from the right side 590 ; 1/b. check if it's viewable from the right side
608 ADDLI .2,sub_a ; avoid thin mess on the side of the display 591 ADDLI .2,sub_a ; avoid thin mess on the side of the display
609 ADDLI .158,sub_b ; load the display offset right side into sub_b 592 ADDLI .158,sub_b ; load the display offset right side into sub_b
610 call subU16 ; sub_c = sub_a - sub_b 593 call subU16 ; sub_c = sub_a - sub_b
611 btfss neg_flag ; xRDr > xA(+2) ? 594 btfss neg_flag ; xRDr > xA(+2) ?
612 return ; NO - done 595 return ; NO - done
613 ; YES - print the bearing lines on the screen 596 ; YES - print the bearing lines on the screen
614 movff xC+0,xCM 597 movff xC+0,xCM
615 bsf compass_bearing_vis ; set visible 598 bsf compass_bearing_vis ; set visible
616 bsf compass_bearing_ahd ; set ahead 599 bsf compass_bearing_ahd ; set ahead
617 return ; done 600 return ; done
618 601
619 TFT_dive_compass_bearing_bp: 602 TFT_dive_compass_bearing_bp:
620 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display 603 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display
621 ; the marker's offset received in sub_b 604 ; the marker's offset received in sub_b
622 ; the xRD180 display offset received in sub_a 605 ; the xRD180 display offset received in sub_a
623 ; xRD180 - xCM 606 ; xRD180 - xCM
624 call subU16 ; sub_c = sub_a - sub_b 607 call subU16 ; sub_c = sub_a - sub_b
625 btfsc neg_flag ; CM > xRD180 ? 608 btfsc neg_flag ; CM > xRD180 ?
626 return ; NO - not on screen, done 609 return ; NO - not on screen, done
627 ; YES - check 160 - (X) 610 ; YES - check 160 - (X)
628 MOVLI .158, sub_a ; 158 to avoid thin mess on the side of the display 611 MOVLI .158, sub_a ; 158 to avoid thin mess on the side of the display
629 MOVII sub_c,sub_b 612 MOVII sub_c,sub_b
630 call subU16 ; sub_c = sub_a - sub_b 613 call subU16 ; sub_c = sub_a - sub_b
631 btfsc neg_flag ; X > 160 ? 614 btfsc neg_flag ; X > 160 ?
632 return ; NO - not on screen, done 615 return ; NO - not on screen, done
633 ; check if not overflow - this sounds like a double check... 616 ; check if not overflow - this sounds like a double check...
634 tstfsz sub_c+1 ; high byte = 0 ? 617 tstfsz sub_c+1 ; high byte = 0 ?
635 return ; NO - sub_c must be > 160 then, done 618 return ; NO - sub_c must be > 160 then, done
636 movlw d'158' 619 movlw d'158' ; YES - load a 158
637 cpfslt sub_c+0 ; low byte < 158 ? 620 cpfslt sub_c+0 ; - low byte < 158 ?
638 return ; NO - done 621 return ; NO - done
639 movff sub_c+0,xCM ; YES to both - print the bearing lines on the screen 622 movff sub_c+0,xCM ; YES - print the bearing lines on the screen
640 bsf compass_bearing_vis 623 bsf compass_bearing_vis ; - flag to show bearing lines
641 return ; done 624 return ; - done
642 625
643 TFT_dive_compass_bearing_dir: 626 TFT_dive_compass_bearing_dir:
644 ; check if bearing to heading, and calculate the direction 627 ; check if bearing to heading, and calculate the direction
645 bcf compass_bearing_eq 628 bcf compass_bearing_eq
646 btfss compass_bearing_vis 629 btfss compass_bearing_vis
650 movff xCM,xA+0 633 movff xCM,xA+0
651 movlw d'80' 634 movlw d'80'
652 cpfseq xA+0 635 cpfseq xA+0
653 bra TFT_dive_compass_bearing_lr 636 bra TFT_dive_compass_bearing_lr
654 bsf compass_bearing_eq 637 bsf compass_bearing_eq
655 bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler 638 bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler
656 639
657 TFT_dive_compass_bearing_lr: 640 TFT_dive_compass_bearing_lr:
658 ; get the bearing virtual display offset 641 ; get the bearing virtual display offset
659 MOVII compass_bearing,xA 642 MOVII compass_bearing,xA
660 ; xA = xA > 292 ? xA : xA+360 643 ; xA = xA > 292 ? xA : xA+360
661 MOVLI .292,sub_a 644 MOVLI .292,sub_a
662 MOVII xA, sub_b 645 MOVII xA, sub_b
663 call subU16 ; sub_c = sub_a - sub_b 646 call subU16 ; sub_c = sub_a - sub_b
664 btfsc neg_flag ; xA > 292 ? 647 btfsc neg_flag ; xA > 292 ?
665 bra TFT_dive_compass_bearing_lr_1 ; YES 648 bra TFT_dive_compass_bearing_lr_1; YES
666 ADDLI .360,xA ; NO - add 360 649 ADDLI .360,xA ; NO - add 360
667 650
668 TFT_dive_compass_bearing_lr_1: 651 TFT_dive_compass_bearing_lr_1:
669 ; 1. calculate whether bearing is to left or to right 652 ; 1. calculate whether bearing is to left or to right
670 bsf compass_bearing_lft ; to the left by default 653 bsf compass_bearing_lft ; to the left by default
671 ; xC: save center value to compare the direction to front value 654 ; xC: save center value to compare the direction to front value
672 MOVII xA,xC 655 MOVII xA,xC
673 ; xB: we need the left side for comparison... left = -180 656 ; xB: we need the left side for comparison... left = -180
674 MOVII xA, sub_a 657 MOVII xA, sub_a
675 MOVLI .180,sub_b 658 MOVLI .180,sub_b
676 call subU16 ; sub_c = sub_a - sub_b 659 call subU16 ; sub_c = sub_a - sub_b
677 MOVII sub_c,xB ; xB has the left side of the 180° distance center 660 MOVII sub_c,xB ; xB has the left side of the 180° distance center
678 ; xA = xRD > (xC+100) ? RD-280 : xRD+80 661 ; xA = xRD > (xC+100) ? RD-280 : xRD+80
679 MOVII xC, sub_a 662 MOVII xC, sub_a
680 ADDLI .100,sub_a 663 ADDLI .100,sub_a
681 MOVII xRD, sub_b 664 MOVII xRD, sub_b
682 call subU16 ; sub_c = sub_a - sub_b 665 call subU16 ; sub_c = sub_a - sub_b
683 btfsc neg_flag ; xRD > xC + 100 ? 666 btfsc neg_flag ; xRD > xC + 100 ?
684 bra TFT_dive_compass_bearing_lr_2 ; YES - xA = xRD - 280 667 bra TFT_dive_compass_bearing_lr_2; YES - xA = xRD - 280
685 ; NO - xA = xRD + 80 668 ; NO - xA = xRD + 80
686 MOVII xRD,xA 669 MOVII xRD,xA
687 ADDLI .80,xA 670 ADDLI .80,xA
688 bra TFT_dive_compass_bearing_lr_c 671 bra TFT_dive_compass_bearing_lr_c
689 672
690 TFT_dive_compass_bearing_lr_2: 673 TFT_dive_compass_bearing_lr_2:
691 MOVII xRD,sub_a 674 MOVII xRD,sub_a
692 MOVLI .280,sub_b 675 MOVLI .280,sub_b
693 call subU16 ; sub_c = sub_a - sub_b 676 call subU16 ; sub_c = sub_a - sub_b
694 MOVII sub_c,xA 677 MOVII sub_c,xA
695 ;bra TFT_dive_compass_bearing_lr_c 678 ;bra TFT_dive_compass_bearing_lr_c
696 679
697 TFT_dive_compass_bearing_lr_c: 680 TFT_dive_compass_bearing_lr_c:
698 ; xB < xA < xC => right, otherwise left (default) 681 ; xB < xA < xC => right, otherwise left (default)
699 MOVII xA,sub_b 682 MOVII xA,sub_b
700 MOVII xB,sub_a 683 MOVII xB,sub_a
701 call subU16 ; sub_c = sub_a - sub_b 684 call subU16 ; sub_c = sub_a - sub_b
702 btfss neg_flag ; xA > xB ? 685 btfss neg_flag ; xA > xB ?
703 bra TFT_dive_compass_ruler ; NO - xB >= xA, keep default left 686 bra TFT_dive_compass_ruler ; NO - xB >= xA, keep default left
704 MOVII xA,sub_a 687 MOVII xA,sub_a
705 MOVII xC,sub_b 688 MOVII xC,sub_b
706 call subU16 ; sub_c = sub_a - sub_b 689 call subU16 ; sub_c = sub_a - sub_b
707 btfss neg_flag ; xC > xA ? 690 btfss neg_flag ; xC > xA ?
708 bra TFT_dive_compass_ruler ; NO - xA >= xC, keep default left 691 bra TFT_dive_compass_ruler ; NO - xA >= xC, keep default left
709 bcf compass_bearing_lft 692 bcf compass_bearing_lft
710 693
711 TFT_dive_compass_ruler: 694 TFT_dive_compass_ruler:
712 ; calculate mod15 for the ticks 695 ; calculate mod15 for the ticks
713 MOVII xRD,xA 696 MOVII xRD,xA
714 MOVLI .15,xB 697 MOVLI .15,xB
715 call div16x16 ; xA/xB=xC with xA+0 as remainder 698 call div16x16 ; xA/xB=xC with xA+0 as remainder
716 ; check the remainder 699 ; check the remainder
717 movlw d'0' 700 movlw d'0'
718 cpfsgt xA+0 ; mod15 > 0 ? 701 cpfsgt xA+0 ; mod15 > 0 ?
719 bra TFT_dive_compass_ruler_1 ; NO - RM = 0 702 bra TFT_dive_compass_ruler_1 ; NO - RM = 0
720 ; YES - RM = 15 - RDmod15 703 ; YES - RM = 15 - RDmod15
721 movlw d'15' 704 movlw d'15'
722 subfwb xA+0,F 705 subfwb xA+0,F
723 TFT_dive_compass_ruler_1: 706 TFT_dive_compass_ruler_1:
724 movff xA+0,lo ; xA+0 holds the RM, store it to 'lo' 707 movff xA+0,lo ; xA+0 holds the RM, store it to 'lo'
725 clrf hi ; initialize DD to zero, store it to 'hi' 708 clrf hi ; initialize DD to zero, store it to 'hi'
726 709
727 TFT_dive_compass_ruler_loop: 710 TFT_dive_compass_ruler_loop:
728 ; 1. check if we run out of the display 711 ; 1. check if we run out of the display
729 movlw d'159' ; looks like 159 works because TFT_box limits the display 712 movlw d'159' ; looks like 159 works because TFT_box limits the display
730 cpfslt lo,1 713 cpfslt lo,1
731 bra TFT_dive_compass_ruler_lend ; xRM >= W 714 bra TFT_dive_compass_ruler_lend ; xRM >= W
732 ; 2. Clear the tick area from DD to RM - in segments to avoid blinking 715 ; 2. Clear the tick area from DD to RM - in segments to avoid blinking
733 ; don't do a clear if we are at 0 (zero) otherwise it will blink 716 ; don't do a clear if we are at 0 (zero) otherwise it will blink
734 ; because of the width underflow 717 ; because of the width underflow
735 movlw d'0' 718 movlw d'0'
736 cpfsgt lo,1 719 cpfsgt lo,1
737 bra TFT_dive_compass_ruler_loop_zz 720 bra TFT_dive_compass_ruler_loop_zz
738 rcall TFT_dive_compass_clr_ruler 721 rcall TFT_dive_compass_clr_ruler
739 TFT_dive_compass_ruler_loop_zz: 722 TFT_dive_compass_ruler_loop_zz:
740 ; 3. Draw the markers @ RM 723 ; 3. Draw the markers @ RM
741 ; we receive RM in lo and DD in hi 724 ; we receive RM in lo and DD in hi
742 movlw dm_custom_compass_tick_top_top
743 movwf win_top
744 movlw dm_custom_compass_tick_height
745 movwf win_height
746 movlw d'2' 725 movlw d'2'
747 movwf win_bargraph 726 movwf win_bargraph ; set with of ticks
748 movwf win_width+0 727 movwf win_width+0
749 clrf win_width+1 728 clrf win_width+1
750 movff lo,win_leftx2 ; 0..159 729 movff lo,win_leftx2 ; 0..159
751 call TFT_standard_color 730 movlw dm_custom_compass_tick_top_top
752 call TFT_box 731 movwf win_top ; set position for upper ticks
732 movlw dm_custom_compass_tick_height
733 movwf win_height ; set hight of ticks
734 call TFT_standard_color ; set color
735 call TFT_box ; draw tick
753 movlw dm_custom_compass_tick_bot_top 736 movlw dm_custom_compass_tick_bot_top
754 movwf win_top 737 movwf win_top ; set position for lower ticks
755 movlw dm_custom_compass_tick_height 738 movlw dm_custom_compass_tick_height
756 movwf win_height 739 movwf win_height ; set hight of ticks
757 call TFT_standard_color ; color in WREG is trashed, must be set again! 740 call TFT_standard_color ; color in WREG is trashed, must be set again!
758 call TFT_box 741 call TFT_box ; draw tick
759 ; 4. If D<82 and RM>79: means we put something over the center line 742 ; 4. If D < 82 and RM > 79: means we put something over the center line,
760 ; redraw the center line 743 ; so redraw the center line
761 movlw d'82' 744 movlw d'82'
762 cpfslt hi,1 745 cpfslt hi,1
763 bra TFT_dive_compass_ruler_loop_zz2 746 bra TFT_dive_compass_ruler_loop_zz2
764 movlw d'79' 747 movlw d'79'
765 cpfsgt lo,1 748 cpfsgt lo,1
766 bra TFT_dive_compass_ruler_loop_zz2 749 bra TFT_dive_compass_ruler_loop_zz2
767 ; enough to print center line as bearing marker is not in the ticker area 750 ; enough to print center line as bearing marker is not in the ticker area
768 movlw color_yellow 751 rcall TFT_dive_compass_c ; draw 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
770 TFT_dive_compass_ruler_loop_zz2: 752 TFT_dive_compass_ruler_loop_zz2:
771 ; 5. set D = RM + 2 : position after the 2px tick 753 ; 5. set D = RM + 2 : position after the 2px tick
772 movff lo,hi 754 movff lo,hi
773 movlw d'2' 755 movlw d'2'
774 addwf hi,F 756 addwf hi,F
776 movlw d'15' 758 movlw d'15'
777 addwf lo,F 759 addwf lo,F
778 ; 7. loop 760 ; 7. loop
779 bra TFT_dive_compass_ruler_loop 761 bra TFT_dive_compass_ruler_loop
780 762
781 TFT_dive_compass_ruler_lend: ; loop end 763 TFT_dive_compass_ruler_lend: ; loop end
782 ; 8. clear the rest of the tick area if D<160 764 ; 8. clear the rest of the tick area if D < 160
783 movlw d'160' 765 movlw d'160'
784 cpfslt hi 766 cpfslt hi
785 bra TFT_dive_compass_ruler_lend2 ; D >= W 767 bra TFT_dive_compass_labels ; D >= W
786 ; 9. position left to end of display to clear the remaining area 768 ; 9. position left to end of display to clear the remaining area
787 movlw d'159' 769 movlw d'159'
788 movwf lo 770 movwf lo
789 ; 10. clear it 771 ; 10. clear it
790 rcall TFT_dive_compass_clr_ruler 772 rcall TFT_dive_compass_clr_ruler
791 773
792 TFT_dive_compass_ruler_lend2: 774 TFT_dive_compass_labels:
793 ; done with the compass ruler, put the labels on the screen 775 ; done with the compass ruler, put the labels on the screen
794 clrf hi ; hi stores the display position 776 clrf hi ; hi stores the display position
795 movff hi,xHI ; bank-safe clear of xHI 777 movff hi,xHI ; bank-safe clear of xHI
796 clrf lo ; lo stores the last item's display position 778 clrf lo ; lo stores the last item's display position
797 movff lo,xLO ; bank-safe clear of xLO 779 movff lo,xLO ; bank-safe clear of xLO
798 MOVLI .219,sub_a ; position of the cardinal 780 MOVLI .219,sub_a ; position of the cardinal
799 MOVII xRD, sub_b ; get the RD back to sub_b 781 MOVII xRD, sub_b ; get the RD back to sub_b
800 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 782 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
801 btfss compass_show_cardinal ; shall show cardinal? 783 btfss compass_show_cardinal ; shall show cardinal?
802 bra dcr_1 ; NO 784 bra dcr_1 ; NO
803 STRCPY_TEXT_PRINT tSW ; YES - print it 785 STRCPY_TEXT_PRINT tSW ; YES - print it
804 dcr_1: 786 dcr_1:
805 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 787 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
806 MOVLI .267,sub_a ; position of the cardinal 788 MOVLI .267,sub_a ; position of the cardinal
807 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 789 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
808 btfss compass_show_cardinal ; shall show cardinal? 790 btfss compass_show_cardinal ; shall show cardinal?
809 bra dcr_2 ; NO 791 bra dcr_2 ; NO
810 STRCPY_TEXT_PRINT tW ; YES - print it 792 STRCPY_TEXT_PRINT tW ; YES - print it
811 dcr_2: 793 dcr_2:
812 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 794 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
813 MOVLI .309,sub_a ; position of the cardinal 795 MOVLI .309,sub_a ; position of the cardinal
814 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 796 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
815 btfss compass_show_cardinal ; shall show cardinal? 797 btfss compass_show_cardinal ; shall show cardinal?
816 bra dcr_3 ; NO 798 bra dcr_3 ; NO
817 STRCPY_TEXT_PRINT tNW ; YES - print it 799 STRCPY_TEXT_PRINT tNW ; YES - print it
818 dcr_3: 800 dcr_3:
819 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 801 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
820 MOVLI .358,sub_a ; position of the cardinal 802 MOVLI .358,sub_a ; position of the cardinal
821 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 803 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
822 btfss compass_show_cardinal ; shall show cardinal? 804 btfss compass_show_cardinal ; shall show cardinal?
823 bra dcr_4 ; NO 805 bra dcr_4 ; NO
824 STRCPY_TEXT_PRINT tN ; YES - print it 806 STRCPY_TEXT_PRINT tN ; YES - print it
825 dcr_4: 807 dcr_4:
826 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 808 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
827 MOVLI .399,sub_a ; position of the cardinal 809 MOVLI .399,sub_a ; position of the cardinal
828 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 810 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
829 btfss compass_show_cardinal ; shall show cardinal? 811 btfss compass_show_cardinal ; shall show cardinal?
830 bra dcr_5 ; NO 812 bra dcr_5 ; NO
831 STRCPY_TEXT_PRINT tNE ; YES - print it 813 STRCPY_TEXT_PRINT tNE ; YES - print it
832 dcr_5: 814 dcr_5:
833 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 815 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
834 MOVLI .448,sub_a ; position of the cardinal 816 MOVLI .448,sub_a ; position of the cardinal
835 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 817 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
836 btfss compass_show_cardinal ; shall show cardinal? 818 btfss compass_show_cardinal ; shall show cardinal?
837 bra dcr_6 ; NO 819 bra dcr_6 ; NO
838 STRCPY_TEXT_PRINT tE ; YES - print it 820 STRCPY_TEXT_PRINT tE ; YES - print it
839 dcr_6: 821 dcr_6:
840 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 822 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
841 MOVLI .489,sub_a ; position of the cardinal 823 MOVLI .489,sub_a ; position of the cardinal
842 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 824 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
843 btfss compass_show_cardinal ; shall show cardinal? 825 btfss compass_show_cardinal ; shall show cardinal?
844 bra dcr_7 ; NO 826 bra dcr_7 ; NO
845 STRCPY_TEXT_PRINT tSE ; YES - print it 827 STRCPY_TEXT_PRINT tSE ; YES - print it
846 dcr_7: 828 dcr_7:
847 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 829 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
848 MOVLI .538,sub_a ; position of the cardinal 830 MOVLI .538,sub_a ; position of the cardinal
849 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 831 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
850 btfss compass_show_cardinal ;shall show cardinal? 832 btfss compass_show_cardinal ; shall show cardinal?
851 bra dcr_8 ; NO 833 bra dcr_8 ; NO
852 STRCPY_TEXT_PRINT tS ; YES - print it 834 STRCPY_TEXT_PRINT tS ; YES - print it
853 dcr_8: 835 dcr_8:
854 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 836 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
855 MOVLI .579,sub_a ; position of the cardinal 837 MOVLI .579,sub_a ; position of the cardinal
856 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 838 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
857 btfss compass_show_cardinal ; shall show cardinal? 839 btfss compass_show_cardinal ; shall show cardinal?
858 bra dcr_9 ; NO 840 bra dcr_9 ; NO
859 STRCPY_TEXT_PRINT tSW ; YES - print it 841 STRCPY_TEXT_PRINT tSW ; YES - print it
860 dcr_9: 842 dcr_9:
861 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 843 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
862 MOVLI .627,sub_a ; position of the cardinal 844 MOVLI .627,sub_a ; position of the cardinal
863 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 845 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
864 btfss compass_show_cardinal ; shall show cardinal? 846 btfss compass_show_cardinal ; shall show cardinal?
865 bra dcr_10 ; NO 847 bra dcr_10 ; NO
866 STRCPY_TEXT_PRINT tW ; YES - print it 848 STRCPY_TEXT_PRINT tW ; YES - print it
867 dcr_10: 849 dcr_10:
868 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 850 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
869 MOVLI .669,sub_a ; position of the cardinal 851 MOVLI .669,sub_a ; position of the cardinal
870 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 852 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
871 btfss compass_show_cardinal ; shall show cardinal? 853 btfss compass_show_cardinal ; shall show cardinal?
872 bra dcr_11 ; NO 854 bra dcr_11 ; NO
873 STRCPY_TEXT_PRINT tNW ; YES - print it 855 STRCPY_TEXT_PRINT tNW ; YES - print it
874 dcr_11: 856 dcr_11:
875 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 857 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
876 MOVLI .718,sub_a ; position of the cardinal 858 MOVLI .718,sub_a ; position of the cardinal
877 rcall TFT_dive_compass_label_proc ; check if the cardinal should be on screen 859 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen
878 btfss compass_show_cardinal ; shall show? 860 btfss compass_show_cardinal ; shall show?
879 bra dcr_12 ; NO 861 bra dcr_12 ; NO
880 STRCPY_TEXT_PRINT tN ; YES - print it 862 STRCPY_TEXT_PRINT tN ; YES - print it
881 dcr_12: 863 dcr_12:
882 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 864 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
883 865
884 TFT_dive_compass_label_end: 866 TFT_dive_compass_label_end:
885 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 867 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
886 ; restore lo and hi for the final cleanup 868 ; restore lo and hi for the final cleanup
887 movff xLO,lo ; xLO and xHI are stored in bank isr_backup 869 movff xLO,lo ; xLO and xHI are stored in bank isr_backup
888 movff xHI,hi 870 movff xHI,hi
889 ; clear the rest of the SQ area if there are more space 871 ; clear the rest of the SQ area if there is more space
890 movlw d'159' 872 movlw d'159'
891 cpfslt hi 873 cpfslt hi
892 bra TFT_dive_compass_label_end2 ; D >= 160, no more space 874 bra TFT_dive_compass_label_end2 ; D >= 160, no more space
893 ; position left to end of display to clear the remaining area 875 ; position left to end of display to clear the remaining area
894 movlw d'158' 876 movlw d'158'
895 movwf lo 877 movwf lo
896 ; clear it 878 ; clear it
897 rcall TFT_dive_compass_clr_label 879 rcall TFT_dive_compass_clr_label
898 TFT_dive_compass_label_end2: 880 TFT_dive_compass_label_end2:
899 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker 881 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker
900 ; do we have bearing set? 882 ; do we have bearing set?
901 btfsc compass_bearing_set ; bearing set? 883 btfsc compass_bearing_set ; bearing set?
902 bra TFT_dive_compass_dir_text ; YES - print the direction (<< or >>) 884 bra TFT_dive_compass_dir_text ; YES - print the direction (<< or >>)
903 rcall TFT_dive_compass_dir_lclr ; NO - clear the area (e.g. we had but removed) 885 rcall TFT_dive_compass_dir_lclr ; NO - clear the area (e.g. we had but removed)
904 rcall TFT_dive_compass_dir_rclr 886 rcall TFT_dive_compass_dir_rclr
905 bra TFT_dive_compass_text 887 bra TFT_dive_compass_text
906 888
907 TFT_dive_compass_dir_text: 889 TFT_dive_compass_dir_text:
908 ; bearing set, but does it point to heading? 890 ; bearing set, but does it point to heading?
909 btfss compass_bearing_eq 891 btfss compass_bearing_eq
910 bra TFT_dive_compass_dir_text_2 ; bearing != heading - go and print the direction 892 bra TFT_dive_compass_dir_text_2 ; bearing != heading - go and print the direction
911 rcall TFT_dive_compass_dir_lclr ; bearing == heading - no need for direction markers 893 rcall TFT_dive_compass_dir_lclr ; bearing == heading - no need for direction markers
912 rcall TFT_dive_compass_dir_rclr 894 rcall TFT_dive_compass_dir_rclr
913 bra TFT_dive_compass_text 895 bra TFT_dive_compass_text
914 896
915 TFT_dive_compass_dir_text_2: 897 TFT_dive_compass_dir_text_2:
916 movlw color_green 898 movlw color_green
917 call TFT_set_color 899 call TFT_set_color
918 btfsc compass_bearing_lft 900 btfsc compass_bearing_lft
919 bra TFT_dive_compass_dir_ldir ; bearing_lft=1, print the left marker 901 bra TFT_dive_compass_dir_ldir ; bearing_lft=1, print the left marker
920 ;TFT_dive_compass_text_rdir: 902 ;TFT_dive_compass_text_rdir:
921 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row-.2 903 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row-.2
922 STRCPY_PRINT ">>" 904 STRCPY_PRINT ">>"
923 rcall TFT_dive_compass_dir_lclr ; do not forget to clear the left 905 rcall TFT_dive_compass_dir_lclr ; do not forget to clear the left
924 bra TFT_dive_compass_text 906 bra TFT_dive_compass_text
925 907
926 TFT_dive_compass_dir_ldir: 908 TFT_dive_compass_dir_ldir:
927 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2 909 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2
928 STRCPY_PRINT "<<" 910 STRCPY_PRINT "<<"
929 rcall TFT_dive_compass_dir_rclr ; do not forget to clear the right 911 rcall TFT_dive_compass_dir_rclr ; do not forget to clear the right
930 ;bra TFT_dive_compass_text 912 ;bra TFT_dive_compass_text
931 913
932 TFT_dive_compass_text: 914 TFT_dive_compass_text:
933 ; Clear some unused space on the right mH 915 ; Clear some unused space on the right mH
934 WIN_BOX_BLACK dm_custom_compass_tick_top_bot+.1,dm_custom_compass_tick_bot_top-.1,.158,.159 ; top, bottom, left, right 916 WIN_BOX_BLACK dm_custom_compass_tick_top_bot+.1,dm_custom_compass_tick_bot_top-.1,.158,.159 ; top, bottom, left, right
935 917
936 ; Text output 918 ; Text output
937 call TFT_standard_color 919 call TFT_standard_color
938 WIN_SMALL dm_custom_compass_head_column, dm_custom_compass_head_row 920 WIN_SMALL dm_custom_compass_head_column, dm_custom_compass_head_row
939 rcall TFT_surface_compass_heading_com ; show "000° N" 921 rcall TFT_surface_compass_heading_com ; show "xxx° N"
940 return 922 return
941 923
942 TFT_dive_compass_dir_lclr: 924 TFT_dive_compass_dir_lclr:
943 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2 925 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2
944 STRCPY_PRINT " " 926 STRCPY_PRINT " "
949 STRCPY_PRINT " " 931 STRCPY_PRINT " "
950 return 932 return
951 933
952 TFT_dive_compass_label_proc: 934 TFT_dive_compass_label_proc:
953 movlw d'14' 935 movlw d'14'
954 movwf up ; cardinal width in px 936 movwf up ; cardinal width in px
955 bcf compass_show_cardinal 937 bcf compass_show_cardinal
956 ; 1/a. check if it's viewable ? sub_a(RP) >= sub_b(RD) ? 938 ; 1/a. check if it's viewable ? sub_a(RP) >= sub_b(RD) ?
957 ; set the carry flag if sub_b(xRD) is equal to or greater than sub_a(xRP): 939 ; set the carry flag if sub_b(xRD) is equal to or greater than sub_a(xRP):
958 MOVII xRD,sub_b 940 MOVII xRD,sub_b
959 call subU16 ; sub_c = sub_a - sub_b 941 call subU16 ; sub_c = sub_a - sub_b
960 btfsc neg_flag ; >= 0 ? 942 btfsc neg_flag ; >= 0 ?
961 return ; NO 943 return ; NO
962 ; store the RO=RP-RD for drawing 944 ; store the RO=RP-RD for drawing
963 MOVII sub_c,xC 945 MOVII sub_c,xC
964 946
965 ; 1/b. check if it's viewable ? sub_a(RP)+up(width) < sub_b(RD)+160 947 ; 1/b. check if it's viewable ? sub_a(RP)+up(width) < sub_b(RD)+160
966 ; if already above, no need to process the rest of the labels 948 ; if already above, no need to process the rest of the labels
967 movff up,WREG ; take care about the width 949 movff up,WREG ; take care about the width
968 addwf sub_a+0,1 950 addwf sub_a+0,1
969 btfsc STATUS, C 951 btfsc STATUS, C
970 incf sub_a+1 952 incf sub_a+1
971 953
972 MOVII xRDr,sub_b 954 MOVII xRDr,sub_b
973 call subU16 ; sub_c = sub_a - sub_b 955 call subU16 ; sub_c = sub_a - sub_b
974 btfss neg_flag ; < 0 ? 956 btfss neg_flag ; < 0 ?
975 bra TFT_dive_compass_label_end ; NO 957 bra TFT_dive_compass_label_end ; NO
976 958
977 ; 2. restore RO=RP-RD from 1/a. 959 ; 2. restore RO=RP-RD from 1/a.
978 movff xC+0,lo 960 movff xC+0,lo
979 961
980 ; 3. Clear the segment from DD(hi) to lo 962 ; 3. Clear the segment from DD(hi) to lo
1003 985
1004 TFT_dive_compass_c_mk: 986 TFT_dive_compass_c_mk:
1005 ; Common task to draw center line and marker 987 ; Common task to draw center line and marker
1006 ; until a proper implementation make it simple: 988 ; until a proper implementation make it simple:
1007 rcall TFT_dive_compass_mk 989 rcall TFT_dive_compass_mk
990 TFT_dive_compass_c:
1008 movlw color_yellow 991 movlw color_yellow
1009 WIN_BOX_COLOR dm_custom_compass_tick_top_top, dm_custom_compass_tick_bot_bot,.80,.81 ; center line in yellow 992 WIN_BOX_COLOR dm_custom_compass_tick_top_top, dm_custom_compass_tick_bot_bot,.80,.81 ; center line in yellow
1010 return 993 return
1011 994
1012 TFT_dive_compass_mk: 995 TFT_dive_compass_mk:
1013 ; draw the bearing on the screen if visible and if we just put something over it 996 ; draw the bearing on the screen if visible and if we just put something over it
1014 btfss compass_bearing_set 997 btfss compass_bearing_set ; bearing set?
1015 return ; bearing_set=0 nothing to display 998 return ; NO - done
1016 999 btfss compass_bearing_vis ; YES - bearing visible?
1017 btfss compass_bearing_vis 1000 return ; NO - bearing set but not visible, done
1018 return ; bearing set but not visible
1019 1001
1020 ; save lo/hi from trashing 1002 ; save lo/hi from trashing
1021 MOVII mpr,xA 1003 MOVII mpr,xA
1022 1004
1023 ; did we just update the marker's position? 1005 ; did we just update the marker's position?
1028 btfss compass_bearing_ahd 1010 btfss compass_bearing_ahd
1029 bra TFT_dive_compass_mk_rear 1011 bra TFT_dive_compass_mk_rear
1030 ;TFT_dive_compass_mk_front: 1012 ;TFT_dive_compass_mk_front:
1031 clrf lo 1013 clrf lo
1032 movff xCM,lo 1014 movff xCM,lo
1033 bsf compass_show_cardinal ; set=green marker 1015 bsf compass_show_cardinal ; set=green marker
1034 rcall TFT_dive_compass_mk_print 1016 rcall TFT_dive_compass_mk_print
1035 bcf compass_show_cardinal 1017 bcf compass_show_cardinal
1036 bra TFT_dive_compass_mk_end 1018 bra TFT_dive_compass_mk_end
1037 1019
1038 TFT_dive_compass_mk_rear: 1020 TFT_dive_compass_mk_rear:
1039 clrf lo 1021 clrf lo
1040 movff xCM,lo 1022 movff xCM,lo
1041 bcf compass_show_cardinal ; set=red marker 1023 bcf compass_show_cardinal ; set=red marker
1042 rcall TFT_dive_compass_mk_print 1024 rcall TFT_dive_compass_mk_print
1043 1025
1044 TFT_dive_compass_mk_end: 1026 TFT_dive_compass_mk_end:
1045 MOVII xA,mpr 1027 MOVII xA,mpr
1046 return 1028 return
1047 1029
1048 TFT_dive_compass_mk_print: 1030 TFT_dive_compass_mk_print:
1049 movlw d'1' 1031 movlw d'1'
1050 cpfsgt lo 1032 cpfsgt lo
1051 bra TFT_dive_compass_mk_print_2 ; lo <= 1, skip the first line 1033 bra TFT_dive_compass_mk_print_2 ; lo <= 1, skip the first line
1052 movlw d'2' 1034 movlw d'2'
1053 subwf lo,0 1035 subwf lo,0
1054 ; movff WREG,win_leftx2 1036 ; movff WREG,win_leftx2
1055 rcall TFT_dive_compass_mk_print_3 1037 rcall TFT_dive_compass_mk_print_3
1056 TFT_dive_compass_mk_print_2: 1038 TFT_dive_compass_mk_print_2:
1205 return 1187 return
1206 tft_compass_cardinal_NW: 1188 tft_compass_cardinal_NW:
1207 STRCAT_TEXT tNW 1189 STRCAT_TEXT tNW
1208 return 1190 return
1209 1191
1192 ;-----------------------------------------------------------------------------
1210 1193
1211 compass_heading_common: 1194 compass_heading_common:
1212 btfss compass_enabled ; compass enabled? 1195 btfss compass_enabled ; compass enabled?
1213 bra compass_heading_common3 ; NO 1196 bra compass_heading_common_zero ; NO
1214 1197
1198 ; Get an averaged new heading
1215 movlw compass_averaging ; number of averaging cycles 1199 movlw compass_averaging ; number of averaging cycles
1216 movwf up ; initialize loop counter 1200 movwf up ; initialize loop counter
1217 compass_heading_common1: 1201 compass_heading_common_1:
1218 call I2C_RX_compass ; test compass 1202 call I2C_RX_compass ; test compass
1219 call I2C_RX_accelerometer ; test accelerometer 1203 call I2C_RX_accelerometer ; test accelerometer
1220 call compass_filter ; filter raw compass + accelerometer readings 1204 call compass_filter ; filter raw compass + accelerometer readings
1221 decfsz up,F ; decrement loop counter, done? 1205 decfsz up,F ; decrement loop counter, done?
1222 bra compass_heading_common1 ; NO - loop 1206 bra compass_heading_common_1 ; NO - loop
1223 1207
1224 call compass ; do compass corrections (C-code) 1208 call compass ; do compass correction (C-code)
1225 banksel common ; back to bank common 1209 banksel common ; back to bank common
1226 1210
1227 MOVII compass_heading_old,sub_a ; transfer old heading to sub16 input 1211 ; Check for calibration and change
1228 MOVII compass_heading_new,sub_b ; transfer new heading to sub16 input 1212 MOVII compass_heading_shown,sub_a ; transfer shown heading to sub_a
1213 MOVII compass_heading_new, sub_b ; transfer new heading to sub_b
1229 1214
1230 btfsc sub_b+1,7 ; valid compass calibration? 1215 btfsc sub_b+1,7 ; valid compass calibration?
1231 bra compass_heading_common3 ; NO 1216 bra compass_heading_common_zero ; NO
1232 1217
1233 call sub16 ; calculate compass_heading_old - compass_heading_new 1218 movf sub_a+0,W ; get shown heading, low byte
1234 btfss neg_flag ; result < 0 ? 1219 cpfseq sub_b+0 ; compare with new heading, low byte, equal?
1235 bra compass_heading_common2 ; NO - test for threshold 1220 bra compass_heading_common_2 ; NO - not equal
1236 MOVII compass_heading_new,sub_a ; YES - subtract the other way round 1221 movf sub_a+1,W ; get shown heading, high byte
1237 MOVII compass_heading_old,sub_b ; - ... 1222 cpfseq sub_b+1 ; compare with new heading, high byte, equal?
1238 call sub16 ; - calculate compass_heading_new - compass_heading_old 1223 bra compass_heading_common_2 ; NO - not equal
1239 1224 return ; YES - done
1240 compass_heading_common2: 1225
1241 MOVII compass_heading_new,compass_heading_old ; copy new to old 1226 compass_heading_common_2:
1242 movlw compass_fast_treshold ; get threshold for fast 1227 ; turn both headings such that compass_heading_new points to 0°
1243 cpfsgt sub_c+0 ; heading difference > compass_fast_treshold ? 1228 call subU16 ; sub_c = compass_heading_shown - compass_heading_new
1244 return ; NO - done 1229 btfss neg_flag ; was compass_heading_new in 1st halve?
1245 MOVII compass_heading_new,compass_heading_shown ; YES - copy heading 1230 bra compass_heading_common_3 ; YES - check where compass_heading_shown is now
1246 return ; - done 1231 MOVLI .360, sub_a ; NO - overturned, need to turn back to match 360°
1247 1232 MOVII sub_c,sub_b ; - move overturned compass_heading_new to sub_b
1248 compass_heading_common3: 1233 call subU16 ; - sub_c = angle between overturned compass_heading_shown and 360°
1249 clrf mpr+0 ; NO - create encoding of 0° 1234 compass_heading_common_3:
1250 clrf mpr+1 ; - ... 1235 ; check if turned compass_heading_shown is in 1st or 2nd halve
1251 MOVII mpr,compass_heading_shown ; - copy to compass_heading_shown 1236 MOVII sub_c,sub_a ; sub_a = turned compass_heading_shown
1252 return ; - done 1237 MOVLI .180, sub_b ; sub_b = begin of 2nd halve
1238 call cmpU16 ; check (turned compass_heading_shown) - 180°
1239 btfss neg_flag ; result negative?
1240 bra compass_heading_common_5 ; NO - in 2nd halve, increment towards compass_heading_new
1241 ;bra compass_heading_common_4 ; YES - in 1st halve, decrement towards compass_heading_new
1242
1243 compass_heading_common_4:
1244 ; decrement compass_heading_shown towards compass_heading_new
1245 rcall compass_heading_stepsize_2 ; calculate step size and put it into sub_b
1246 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a
1247 call subU16 ; decrement heading: sub_c = compass_heading_shown - step size
1248 btfss neg_flag ; did an under-run occur?
1249 bra compass_heading_common_6 ; NO - store result
1250 MOVLI .360, sub_a ; YES - wrap around 360°
1251 MOVII sub_c,sub_b ; - transfer decrement result to sub_b
1252 call subU16 ; - wrap decrement result around
1253 bra compass_heading_common_6 ; - store wrapped result
1254
1255
1256 compass_heading_common_5:
1257 ; increment compass_heading_shown towards compass_heading_new
1258 rcall compass_heading_stepsize_1 ; calculate step size and put it into sub_b
1259 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a
1260 call addU16 ; increment heading: sub_c = compass_heading_shown + step size
1261 MOVII sub_c,sub_a ; transfer increment result to sub_a
1262 MOVLI .360, sub_b ; load wrap-around threshold
1263 call subU16 ; calculate if over-run occurred
1264 btfss neg_flag ; did an over-run occur?
1265 bra compass_heading_common_6 ; YES - store already wrapped-around result
1266 MOVII sub_a,sub_c ; NO - retrieve former straight increment result
1267 bra compass_heading_common_6 ; - store wrapped result
1268
1269 compass_heading_common_zero:
1270 CLRI sub_c ; set heading to 0°
1271 ;bra compass_heading_common_6 ; store heading
1272
1273 compass_heading_common_6:
1274 MOVII sub_c,compass_heading_shown ; store new shown heading
1275 return ; done
1276
1277 compass_heading_stepsize_1:
1278 ; turn heading difference (180...359) into a step size
1279 MOVLI .360, sub_a ; load 360°
1280 MOVII sub_c,sub_b ; load difference
1281 call subU16 ; sub_c = 360 - difference (i.e. 1...180 now)
1282 compass_heading_stepsize_2:
1283 ; turn heading difference (1...180) into a step size
1284 bcf STATUS,C ; clear carry
1285 rrcf sub_c+0 ; heading difference /= 2
1286 bcf STATUS,C ; clear carry
1287 rrcf sub_c+0 ; heading difference /= 2, total /= 4 now
1288 incf sub_c+0,f ; final += 1 to have one increment / decrement at least
1289 MOVII sub_c,sub_b ; transfer result to sub_b
1290 return
1291
1253 1292
1254 ENDIF ; _compass 1293 ENDIF ; _compass
1255 1294
1256 END 1295 END