Mercurial > public > hwos_code

comparison src/compass_ops.asm @ 256:5b4ef0b9090d

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
place compass display code into compass_ops.asm
author heinrichsweikamp
date Mon, 23 Mar 2015 18:02:40 +0100
parents a17359244d93
children 5dd0f39d05d4
comparison
equal deleted inserted replaced
255:ad62dff7739a 256:5b4ef0b9090d
4 #include "isr.inc" 4 #include "isr.inc"
5 #include "tft.inc" 5 #include "tft.inc"
6 #include "strings.inc" 6 #include "strings.inc"
7 #include "wait.inc" ; speed_* 7 #include "wait.inc" ; speed_*
8 #include "surfmode.inc" 8 #include "surfmode.inc"
9 #include "divemode.inc"
10 #include "math.inc"
11 #include "convert.inc"
9 12
10 13
11 ; Make sure symbols from the .inc are available to the C code: 14 ; Make sure symbols from the .inc are available to the C code:
12 ; Filtered data 15 ; Filtered data
13 global compass_DX_f, compass_DY_f, compass_DZ_f 16 global compass_DX_f, compass_DY_f, compass_DZ_f
210 clrf opt_compass_gain ; Yes, keep at zero 213 clrf opt_compass_gain ; Yes, keep at zero
211 214
212 banksel common 215 banksel common
213 call I2C_init_accelerometer 216 call I2C_init_accelerometer
214 call I2C_init_compass_fast 217 call I2C_init_compass_fast
215 call TFT_compass_show_gain ; Show the current compass gain 218 rcall TFT_compass_show_gain ; Show the current compass gain
216 219
217 WAITMS d'100' 220 WAITMS d'100'
218 221
219 clrf timeout_counter2 222 clrf timeout_counter2
220 clrf timeout_counter3 223 clrf timeout_counter3
318 bra compass_calibration_loop2 ; no, loop here 321 bra compass_calibration_loop2 ; no, loop here
319 322
320 movlw .60 323 movlw .60
321 call timeout_testmode ; check timeout 324 call timeout_testmode ; check timeout
322 movlw .60 325 movlw .60
323 call TFT_show_timeout_testmode ; Show the timeout 326 rcall TFT_show_timeout_testmode ; Show the timeout
324 327
325 bcf onesecupdate ; clear flag 328 bcf onesecupdate ; clear flag
326 329
327 bra compass_calibration_loop2 ; loop here 330 bra compass_calibration_loop2 ; loop here
328 331
334 bcf sleepmode ; Clear the flag before exiting to surfacemode 337 bcf sleepmode ; Clear the flag before exiting to surfacemode
335 movlw .6 338 movlw .6
336 movwf customview_surfmode ; Set to compass view... 339 movwf customview_surfmode ; Set to compass view...
337 goto surfloop ; ...and exit 340 goto surfloop ; ...and exit
338 341
342 global TFT_compass_fast
343 TFT_compass_fast:
344 WIN_TINY .20,.50
345 STRCPY "X:"
346 movff compass_DX+0,lo
347 movff compass_DX+1,hi
348 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
349 output_16
350 STRCAT " Y:"
351 movff compass_DY+0,lo
352 movff compass_DY+1,hi
353 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
354 output_16
355 STRCAT " Z:"
356 movff compass_DZ+0,lo
357 movff compass_DZ+1,hi
358 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
359 output_16
360 STRCAT_PRINT " "
361 return
362
363 TFT_show_timeout_testmode: ; With timeout in WREG...
364 movwf hi
365 WIN_TINY .20,.68
366 STRCPY "T:"
367 movf timeout_counter2,W ; current timeout
368 subwf hi,W ; subtract from timeout value
369 addlw .1 ; +1
370 movwf lo
371 bsf leftbind
372 output_8 ; Display timeout
373 bcf leftbind
374 STRCAT_PRINT "s "
375 return
376
377
378 TFT_compass_show_gain: ; Show the current compass gain
379 movff opt_compass_gain,lo ; 0-7 (230LSB/Gauss to 1370LSB/Gaus)
380 tstfsz lo
381 return ; Do not show unless gain=0
382 WIN_TINY .20,.86
383 STRCPY_TEXT tCompassGain
384 movff opt_compass_gain,lo ; 0-7 (230LSB/Gauss to 1370LSB/Gaus)
385 bsf leftbind
386 output_8
387 bcf leftbind
388 STRCAT_PRINT "!"
389 return
390
391 global TFT_surface_compass_mask
392 TFT_surface_compass_mask:
393 WIN_SMALL surf_compass_mask_column,surf_compass_mask_row
394 call TFT_standard_color
395 STRCPY_TEXT_PRINT tHeading ; Heading:
396 return
397
398 global TFT_dive_compass_mask
399 TFT_dive_compass_mask:
400 WIN_FRAME_STD dive_compass_graph_row, dive_compass_graph_row+dive_compass_graph_height, .0, .159
401 return
402
403 global TFT_surface_compass_heading
404 TFT_surface_compass_heading:
405 rcall compass_heading_common
406 WIN_STD surf_compass_head_column,surf_compass_head_row
407 call TFT_standard_color
408 TFT_surface_compass_heading_com: ; Show "000° N"
409 movff compass_heading+0,lo
410 movff compass_heading+1,hi
411 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
412
413 ; Shown and actual identical?
414 movff compass_heading_shown+0,WREG
415 cpfseq lo
416 bra TFT_surface_compass_heading_com1 ; Not equal
417 movff compass_heading_shown+1,WREG
418 cpfseq hi
419 bra TFT_surface_compass_heading_com1 ; Not equal
420 bra TFT_surface_compass_heading_com3 ; equal, skip smoothing
421
422 TFT_surface_compass_heading_com1:
423 movff lo,sub_a+0
424 movff hi,sub_a+1
425 movff compass_heading_shown+0,sub_b+0
426 movff compass_heading_shown+1,sub_b+1
427 call subU16
428 btfsc neg_flag
429 bra TFT_surface_compass_heading_com2 ; shown > actual
430 ; shown < actual
431 banksel compass_heading_shown
432 infsnz compass_heading_shown+0,F
433 incf compass_heading_shown+1,F ; +1
434 bra TFT_surface_compass_heading_com3
435
436 TFT_surface_compass_heading_com2:
437 banksel compass_heading_shown
438 movlw d'1'
439 subwf compass_heading_shown+0,F
440 movlw d'0'
441 subwfb compass_heading_shown+1,F ; -1
442
443 TFT_surface_compass_heading_com3:
444 banksel common
445 movff compass_heading_shown+0,lo
446 movff compass_heading_shown+1,hi
447 bsf leftbind
448 output_16dp .2 ; Result is "0.000"
449 bcf leftbind
450 ; rearrange figures to "000"
451 movff buffer+2,buffer+0
452 movff buffer+3,buffer+1
453 movff buffer+4,buffer+2
454 lfsr FSR2,buffer+3
455 STRCAT "° "
456 rcall tft_compass_cardinal ; Add cardinal and ordinal to POSTINC2
457 clrf WREG
458 movff WREG,buffer+.7 ; limit to 7 chars
459 STRCAT_PRINT ""
460 return
461
462 global TFT_dive_compass_heading
463 TFT_dive_compass_heading:
464 rcall compass_heading_common
465 ; ; ToDo - these are for development only, hardcoding the bearing position
466 ; ; 244° : SW - W
467 ; movlw low(d'244')
468 ; movff WREG,compass_bearing+0
469 ; movlw high(d'244')
470 ; movff WREG,compass_bearing+1
471
472 movff compass_heading_shown+0,xA+0
473 movff compass_heading_shown+1,xA+1
474 ; xRD and xRDlft
475 ; 1. 160° viewing angle: +360 offset if xA<=292; for non-negative scale
476 movlw high(d'292')
477 movff WREG,sub_a+1
478 movlw low(d'292')
479 movff WREG,sub_a+0
480 movff xA+1,sub_b+1
481 movff xA+0,sub_b+0
482 call subU16 ; sub_c = sub_a - sub_b
483 btfsc neg_flag ; xA>292
484 bra TFT_dive_compass_heading_1 ;yes
485 ; no, xA<=292
486 movlw high(d'360')
487 addwf xA+1,1
488 movlw low(d'360')
489 addwf xA+0,1
490 btfsc STATUS,C
491 incf xA+1
492 TFT_dive_compass_heading_1:
493 ; 2. -80: left pixel offset from the center
494 movlw low( d'80' )
495 subwf xA+0,1
496 btfss STATUS,C
497 decf xA+1
498 ; 3. save it to xRD
499 movff xA+0,xRD+0
500 movff xA+1,xRD+1
501 ; 4. add 160 (display px width)
502 movlw high(d'160')
503 addwf xA+1,1
504 movlw low(d'160')
505 addwf xA+0,1
506 btfsc STATUS,C
507 incf xA+1
508 ; 5. save it to xRDr
509 movff xA+0,xRDr+0
510 movff xA+1,xRDr+1
511
512 ; ; Bearing ?
513 ; ; We can skip this xRD180 calculation if no bearing is set
514 ; bcf compass_bearing_set
515 ; movff compass_bearing+0,sub_a+0
516 ; movff compass_bearing+1,sub_a+1
517 ; movlw d'0'
518 ; cpfseq sub_a+1
519 ; bra TFT_dive_compass_bearing ; something set, calculate xRD180
520 ; movlw d'0'
521 ; cpfseq sub_a+0
522 ; bra TFT_dive_compass_bearing ; something set, calculate xRD180
523 ; bra TFT_dive_compass_ruler ; no value in the bearing, skip calc
524 ;
525 ;TFT_dive_compass_bearing:
526 ; bsf compass_bearing_set
527
528 btfss compass_bearing_set
529 bra TFT_dive_compass_ruler ; no value in the bearing, skip calc
530
531 ; we have bearing set, we will need xRD180 calculated
532 ; xRD180 is xRDr-180
533 movff xRDr+1,sub_a+1
534 movff xRDr+0,sub_a+0
535 movlw high(d'180')
536 movff WREG,sub_b+1
537 movlw low(d'180')
538 movff WREG,sub_b+0
539 call subU16 ; sub_c = sub_a - sub_b
540 movff sub_c+1,xRD180+1
541 movff sub_c+0,xRD180+0
542
543 ; get the bearing virtual display offset, store it to divA
544 movff compass_bearing+0,xA+0
545 movff compass_bearing+1,xA+1
546 ; divA =IF (U10>292;U10;U10+360)
547 movlw high(d'292')
548 movff WREG,sub_a+1
549 movlw low(d'292')
550 movff WREG,sub_a+0
551 movff xA+1,sub_b+1
552 movff xA+0,sub_b+0
553 call subU16 ; sub_c = sub_a - sub_b
554 btfsc neg_flag ; xA>292
555 bra TFT_dive_compass_bearing_1 ;yes
556 ; no, xA<=292
557 movlw high(d'360')
558 addwf xA+1,1
559 movlw low(d'360')
560 addwf xA+0,1
561 btfsc STATUS,C
562 incf xA+1
563 ; save it for the direction (<< or >>) calculation
564 movff xA+1,divA+1
565 movff xA+0,divA+0
566
567 TFT_dive_compass_bearing_1:
568 ; calculate bearing position and visibility (ahead or behind)
569 bcf compass_bearing_vis ; default is not-visibly
570 bcf compass_bearing_ahd ; default is behind
571 ; check if it's ahead
572 ; load the bearing offset into sub_a
573 movff divA+1,sub_a+1
574 movff divA+0,sub_a+0
575 ; load the display offset back to sub_b
576 movff xRD+0,sub_b+0
577 movff xRD+1,sub_b+1
578 rcall TFT_dive_compass_bearing_ap
579 ;test if we found it
580 btfsc compass_bearing_vis
581 bra TFT_dive_compass_bearing_dir
582
583 ; check if it's ahead with a furr turn
584 ; load the bearing offset into sub_a
585 movff divA+1,sub_a+1
586 movff divA+0,sub_a+0
587 ; load the display offset back to sub_b
588 movff xRD+0,sub_b+0
589 movff xRD+1,sub_b+1
590 movlw high(d'360')
591 addwf sub_b+1,1
592 movlw low(d'360')
593 addwf sub_b+0,1
594 btfsc STATUS,C
595 incf sub_b+1
596 rcall TFT_dive_compass_bearing_ap
597 ;test if we found it
598 btfsc compass_bearing_vis
599 bra TFT_dive_compass_bearing_dir
600
601 ; marker is not ahead of us, check if it's behind us
602 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display
603 ; load the display offset back to sub_a
604 movff xRD180+0,sub_a+0
605 movff xRD180+1,sub_a+1
606 ; load the marker's offset into sub_b
607 movff divA+0,sub_b+0
608 movff divA+1,sub_b+1
609 rcall TFT_dive_compass_bearing_bp
610 ;test if we found it
611 btfsc compass_bearing_vis
612 bra TFT_dive_compass_bearing_dir
613
614 ;check if it's behind with the lower turn
615 movff xRD180+0,sub_a+0
616 movff xRD180+1,sub_a+1
617 movlw high(d'360')
618 addwf sub_a+1,1
619 movlw low(d'360')
620 addwf sub_a+0,1
621 btfsc STATUS,C
622 incf sub_a+1
623 ; load the marker's offset into sub_b
624 movff divA+0,sub_b+0
625 movff divA+1,sub_b+1
626 rcall TFT_dive_compass_bearing_bp
627 ;test if we found it
628 btfsc compass_bearing_vis
629 bra TFT_dive_compass_bearing_dir
630
631 ; check if it's behind with the upper turn
632 movff divA+1,sub_b+1
633 movff divA+0,sub_b+0
634 movlw high(d'360')
635 addwf sub_b+1,1
636 movlw low(d'360')
637 addwf sub_b+0,1
638 btfsc STATUS,C
639 incf sub_b+1
640 rcall TFT_dive_compass_bearing_bp
641 bra TFT_dive_compass_bearing_dir
642
643 TFT_dive_compass_bearing_ap:
644 ; xCM received in sub_a
645 ; xRD received in sub_b
646 ; 1/a. check if it's viewable from the left side
647 call subU16 ; sub_c = sub_a - sub_b
648 btfsc neg_flag ; xRD>divA
649 return ;no,
650 ; yes, store the RO=RP-RD for drawing
651 movff sub_c+0,xC+0
652 movff sub_c+1,xC+1
653 ; 1/b. check if it's viewable from the right side?
654 movlw d'2' ; avoid thin mess on the side of the display
655 addwf sub_a+0,1
656 btfsc STATUS, C
657 incf sub_a+1
658 ; load the display offset right side into sub_b
659 movlw high(d'160')
660 addwf sub_b+1,1
661 movlw low(d'160')
662 addwf sub_b+0,1
663 btfsc STATUS,C
664 incf sub_b+1
665 call subU16 ; sub_c = sub_a - sub_b
666 btfss neg_flag ; xRDr>xA(+2)
667 return ; no,
668 ; print the bearing lines on the screen
669 movff xC+0,xCM
670 bsf compass_bearing_vis ; set visible
671 bsf compass_bearing_ahd ; set ahead
672 return ; done,
673
674 TFT_dive_compass_bearing_bp:
675 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display
676 ; the marker's offset received in sub_b
677 ; the xRD180 display offset received in sub_a
678 ; xRD180 - xCM
679 call subU16 ; sub_c = sub_a - sub_b
680 btfsc neg_flag ; CM>xRD180
681 return ; no, not on screen
682 ; 160 - (X)
683 movlw high(d'160')
684 movff WREG,sub_a+1
685 movlw low(d'160')
686 movff WREG,sub_a+0
687 movff sub_c+1,sub_b+1
688 movff sub_c+0,sub_b+0
689 call subU16 ; sub_c = sub_a - sub_b
690 btfsc neg_flag ; X>160
691 return ; no, not on screen
692 ; check if not overflow - this sounds a double check...
693 movlw d'1'
694 cpfslt sub_c+1
695 return ; high set, >160
696 movlw d'160'
697 cpfslt sub_c+0
698 return ; low >160
699 ; print the bearing lines on the screen
700 movff sub_c+0,xCM
701 bsf compass_bearing_vis
702 return ; done
703
704 TFT_dive_compass_bearing_dir:
705 ; check if bearing to heading, and calculate the direction
706 bcf compass_bearing_eq
707 btfss compass_bearing_vis
708 bra TFT_dive_compass_bearing_lr
709 btfss compass_bearing_ahd
710 bra TFT_dive_compass_bearing_lr
711 movff xCM,xA+0
712 movlw d'80'
713 cpfseq xA+0
714 bra TFT_dive_compass_bearing_lr
715 bsf compass_bearing_eq
716 bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler
717
718 TFT_dive_compass_bearing_lr:
719 ; 1. calculate whether bearing is to left or to right
720 bsf compass_bearing_lft ; to the left by default
721 ; get the bearing offset back
722 movff divA+1,xA+1
723 movff divA+0,xA+0
724 ; xC: save center value to compare the direction to front value
725 movff xA+1,xC+1
726 movff xA+0,xC+0
727 ; xB: we need the left side for comparism... left = -180
728 movff xA+1,sub_a+1
729 movff xA+0,sub_a+0
730 movlw high(d'180')
731 movff WREG,sub_b+1
732 movlw low(d'180')
733 movff WREG,sub_b+0
734 call subU16 ; sub_c = sub_a - sub_b
735 movff sub_c+1,xB+1 ; xB has the left side of the 180° distance center
736 movff sub_c+0,xB+0
737 ; xA = IF(xRD>(xC+100);xRD-280;xRD+80)
738 movff xC+1,sub_a+1
739 movff xC+0,sub_a+0
740 movlw d'100'
741 addwf sub_a+0,1
742 btfsc STATUS,C
743 incf sub_a+1
744 movff xRD+1,sub_b+1
745 movff xRD+0,sub_b+0
746 call subU16 ; sub_c = sub_a - sub_b
747 btfsc neg_flag ; xRD>xC+100
748 bra TFT_dive_compass_bearing_lr_2 ; yes, xA=xRD-280
749 ; no, xA = xRD+80
750 movff xRD+1,xA+1
751 movff xRD+0,xA+0
752 movlw d'80'
753 addwf xA+0,1
754 btfsc STATUS,C
755 incf xA+1
756 bra TFT_dive_compass_bearing_lr_c
757
758 TFT_dive_compass_bearing_lr_2:
759 ; xA=xRD-280
760 movff xRD+1,sub_a+1
761 movff xRD+0,sub_a+0
762 movlw high(d'280')
763 movff WREG,sub_b+1
764 movlw low(d'280')
765 movff WREG,sub_b+0
766 call subU16 ; sub_c = sub_a - sub_b
767 movff sub_c+1,xA+1
768 movff sub_c+0,xA+0
769 ;bra TFT_dive_compass_bearing_lr_c
770
771 TFT_dive_compass_bearing_lr_c:
772 ; xB < xA < xC => right, otherwise left (default)
773 movff xA+1,sub_b+1
774 movff xA+0,sub_b+0
775 movff xB+1,sub_a+1
776 movff xB+0,sub_a+0
777 call subU16 ; sub_c = sub_a - sub_b
778 btfss neg_flag ; xA>xB ?
779 bra TFT_dive_compass_ruler ; No, xB >= xA, keep default left
780 movff xA+1,sub_a+1
781 movff xA+0,sub_a+0
782 movff xC+1,sub_b+1
783 movff xC+0,sub_b+0
784 call subU16 ; sub_c = sub_a - sub_b
785 btfss neg_flag ; xC>xA ?
786 bra TFT_dive_compass_ruler ; No, xA >= xC, keep default left
787 bcf compass_bearing_lft
788
789 TFT_dive_compass_ruler:
790 ; calculate mod15 for the ticks
791 movff xRD+0,xA+0
792 movff xRD+1,xA+1
793 movlw d'15'
794 movwf xB+0
795 clrf xB+1
796 call div16x16 ;xA/xB=xC with xA+0 as remainder
797 ; check xA+0, it has the remainder
798 movlw d'0'
799 cpfsgt xA+0 ; mod15 > 0
800 bra TFT_dive_compass_ruler_1 ; no, RM = 0
801 ; yes RM = 15 - RDmod15
802 movlw d'15'
803 subfwb xA+0,1
804 TFT_dive_compass_ruler_1:
805 ; xA+0 holds the RM, store it to 'lo'
806 movff xA+0,lo
807 ; init DD to zero, store it to 'hi'
808 movlw d'0'
809 movff WREG,hi
810
811 TFT_dive_compass_ruler_loop:
812 ; 1. check if we run of from the display
813 movlw d'160' ; Looks like 160 works because TFT_box limits the dispay
814 cpfslt lo,1
815 bra TFT_dive_compass_ruler_lend ; xRM >= W
816 ; 2. Clear the tick area from DD to RM - in segments to avoid blinking
817 ; don't do a clear if we are at 0 (zero) otherwise it will blink
818 ; because of the width underflow
819 movlw d'0'
820 cpfsgt lo,1
821 bra TFT_dive_compass_ruler_loop_zz
822 rcall TFT_dive_compass_clr_ruler
823 TFT_dive_compass_ruler_loop_zz:
824 ; 3. Draw the markers @ RM
825 rcall TFT_dive_compass_ruler_print
826 ; 4. If D<82 and RM>79: means we put something over the center line
827 ; redraw the center line
828 movlw d'82'
829 cpfslt hi,1
830 bra TFT_dive_compass_ruler_loop_zz2
831 movlw d'79'
832 cpfsgt lo,1
833 bra TFT_dive_compass_ruler_loop_zz2
834 rcall TFT_dive_compass_c_mk
835 TFT_dive_compass_ruler_loop_zz2:
836 ; 5. set D = RM + 2 : position after the 2px tick
837 movff lo,hi
838 movlw d'2'
839 addwf hi,1
840 ; 6. set RM = RM + 15 : position to the next tick
841 movlw d'15'
842 addwf lo,1
843 ; 7. loop
844 bra TFT_dive_compass_ruler_loop
845
846 TFT_dive_compass_ruler_lend: ; loop end
847 ; 8. clear the rest of the tick area if D<160
848 movlw d'160'
849 cpfslt hi,1
850 bra TFT_dive_compass_ruler_lend2 ; D >= W
851 ; 9. position left to end of display to clear the remaining area
852 movlw d'160'
853 movwf lo
854 ; 10. clear it
855 rcall TFT_dive_compass_clr_ruler
856
857 TFT_dive_compass_ruler_lend2:
858 rcall TFT_dive_compass_c_mk
859 ; done with the compass ruler, put the labels on the screen
860 ; get the RD abck to sub_b
861 movff xRD+0,sub_b+0
862 movff xRD+1,sub_b+1
863 ; hi stores the display position
864 movlw d'0'
865 movwf hi
866 ; lo stores the last item's display position
867 movlw d'0'
868 movwf lo
869 bcf print_compass_label
870
871 movlw d'14'
872 movwf up ; up stores the width of hte label
873 movlw low( d'219' ) ; position of the label
874 movwf sub_a+0
875 movlw high( d'219' )
876 movwf sub_a+1
877 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
878 btfss print_compass_label ; Yes?
879 bra dcr_1
880 STRCPY_TEXT_PRINT tSW ; yes - print it
881 dcr_1:
882 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
883
884 movlw d'7'
885 movwf up ; up stores the width of hte label
886 movlw low( d'267' ) ; position of the label
887 movwf sub_a+0
888 movlw high( d'267' )
889 movwf sub_a+1
890 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
891 btfss print_compass_label ; Yes?
892 bra dcr_2
893 STRCPY_TEXT_PRINT tW ; yes - print it
894 dcr_2:
895 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
896
897 movlw d'14'
898 movwf up ; up stores the width of hte label
899 movlw low( d'309' ) ; position of the label
900 movwf sub_a+0
901 movlw high( d'309' )
902 movwf sub_a+1
903 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
904 btfss print_compass_label ; Yes?
905 bra dcr_3
906 STRCPY_TEXT_PRINT tNW ; yes - print it
907 dcr_3:
908 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
909
910 movlw d'6'
911 movwf up ; up stores the width of hte label
912 movlw low( d'358' ) ; position of the label
913 movwf sub_a+0
914 movlw high( d'358' )
915 movwf sub_a+1
916 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
917 btfss print_compass_label ; Yes?
918 bra dcr_4
919 STRCPY_TEXT_PRINT tN ; yes - print it
920 dcr_4:
921 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
922
923 movlw d'13'
924 movwf up ; up stores the width of hte label
925 movlw low( d'399' ) ; position of the label
926 movwf sub_a+0
927 movlw high( d'399' )
928 movwf sub_a+1
929 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
930 btfss print_compass_label ; Yes?
931 bra dcr_5
932 STRCPY_TEXT_PRINT tNE ; yes - print it
933 dcr_5:
934 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
935
936 movlw d'6'
937 movwf up ; up stores the width of hte label
938 movlw low( d'448' ) ; position of the label
939 movwf sub_a+0
940 movlw high( d'448' )
941 movwf sub_a+1
942 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
943 btfss print_compass_label ; Yes?
944 bra dcr_6
945 STRCPY_TEXT_PRINT tE ; yes - print it
946 dcr_6:
947 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
948
949 movlw d'13'
950 movwf up ; up stores the width of hte label
951 movlw low( d'489' ) ; position of the label
952 movwf sub_a+0
953 movlw high( d'489' )
954 movwf sub_a+1
955 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
956 btfss print_compass_label ; Yes?
957 bra dcr_7
958 STRCPY_TEXT_PRINT tSE ; yes - print it
959 dcr_7:
960 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
961
962 movlw d'6'
963 movwf up ; up stores the width of hte label
964 movlw low( d'538' ) ; position of the label
965 movwf sub_a+0
966 movlw high( d'538' )
967 movwf sub_a+1
968 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
969 btfss print_compass_label ; Yes?
970 bra dcr_8
971 STRCPY_TEXT_PRINT tS ; yes - print it
972 dcr_8:
973 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
974
975 movlw d'14'
976 movwf up ; up stores the width of hte label
977 movlw low( d'579' ) ; position of the label
978 movwf sub_a+0
979 movlw high( d'579' )
980 movwf sub_a+1
981 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
982 btfss print_compass_label ; Yes?
983 bra dcr_9
984 STRCPY_TEXT_PRINT tSW ; yes - print it
985 dcr_9:
986 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
987
988 movlw d'7'
989 movwf up ; up stores the width of hte label
990 movlw low( d'627' ) ; position of the label
991 movwf sub_a+0
992 movlw high( d'627' )
993 movwf sub_a+1
994 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
995 btfss print_compass_label ; Yes?
996 bra dcr_10
997 STRCPY_TEXT_PRINT tW ; yes - print it
998 dcr_10:
999 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
1000
1001 movlw d'14'
1002 movwf up ; up stores the width of hte label
1003 movlw low( d'669' ) ; position of the label
1004 movwf sub_a+0
1005 movlw high( d'669' )
1006 movwf sub_a+1
1007 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
1008 btfss print_compass_label ; Yes?
1009 bra dcr_11
1010 STRCPY_TEXT_PRINT tNW ; yes - print it
1011 dcr_11:
1012 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
1013
1014 movlw d'6'
1015 movwf up ; up stores the width of hte label
1016 movlw low( d'718' ) ; position of the label
1017 movwf sub_a+0
1018 movlw high( d'718' )
1019 movwf sub_a+1
1020 rcall TFT_dive_compass_label_proc ; check if the label should be on screen
1021 btfss print_compass_label ; Yes?
1022 bra dcr_12
1023 STRCPY_TEXT_PRINT tN ; yes - print it
1024 dcr_12:
1025 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
1026
1027 TFT_dive_compass_label_end:
1028 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
1029 ; restore lo and hi for the final cleanup
1030 movff xLO,lo
1031 movff xHI,hi
1032 ; clear the rest of the SQ area if there are more space
1033 movlw d'160'
1034 cpfslt hi
1035 bra TFT_dive_compass_label_end2 ; D >= 160, no more space
1036 ; position left to end of display to clear the remaining area
1037 movlw d'160'
1038 movff WREG,lo
1039 ; clear it
1040 rcall TFT_dive_compass_clr_label
1041 TFT_dive_compass_label_end2:
1042 rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker
1043 ; do we have bearing set?
1044 btfsc compass_bearing_set
1045 bra TFT_dive_compass_dir_text ; bearing_set=1 - go and print the dir (<< or >>)
1046 rcall TFT_dive_compass_dir_lclr ; no, clear the area (e.g. we had but removed)
1047 rcall TFT_dive_compass_dir_rclr
1048 bra TFT_dive_compass_text
1049
1050 TFT_dive_compass_dir_text:
1051 ; bearing set, but does it point to heading?
1052 btfss compass_bearing_eq
1053 bra TFT_dive_compass_dir_text_2 ; bearing != heading - go and print the dir
1054 rcall TFT_dive_compass_dir_lclr ; bearing = heading, no need for direction markers
1055 rcall TFT_dive_compass_dir_rclr
1056 bra TFT_dive_compass_text
1057
1058 TFT_dive_compass_dir_text_2:
1059 btfsc compass_bearing_lft
1060 bra TFT_dive_compass_dir_ldir ; bearing_lft=1, print the left marker
1061 ;TFT_dive_compass_text_rdir:
1062 movlw color_green
1063 call TFT_set_color
1064 WIN_SMALL dive_compass_rdir_column,dive_compass_head_row
1065 STRCPY_PRINT ">>"
1066 ; do not forget to clear the left
1067 rcall TFT_dive_compass_dir_lclr
1068 bra TFT_dive_compass_text
1069
1070 TFT_dive_compass_dir_ldir:
1071 movlw color_green
1072 call TFT_set_color
1073 WIN_SMALL dive_compass_ldir_column,dive_compass_head_row
1074 STRCPY_PRINT "<<"
1075 ; do not forget to clear the right
1076 rcall TFT_dive_compass_dir_rclr
1077 ;bra TFT_dive_compass_text
1078
1079 TFT_dive_compass_text:
1080 ; Text output
1081 call TFT_standard_color
1082 WIN_SMALL dive_compass_head_column,dive_compass_head_row
1083 rcall TFT_surface_compass_heading_com ; Show "000° N"
1084 return
1085
1086 TFT_dive_compass_dir_lclr:
1087 WIN_SMALL dive_compass_ldir_column,dive_compass_head_row
1088 STRCPY_PRINT " "
1089 return
1090
1091 TFT_dive_compass_dir_rclr:
1092 WIN_SMALL dive_compass_rdir_column,dive_compass_head_row
1093 STRCPY_PRINT " "
1094 return
1095
1096 TFT_dive_compass_label_proc:
1097 ; Input:
1098 ; xHI: DD - display'a current position
1099 ; xRD: RD - ruler display offset
1100 ; sub_a: RP - item's ruler display offset
1101 ; get the RD abck to sub_b
1102 movff xHI,hi
1103 bcf print_compass_label
1104 ; 1/a. check if it's viewable ? sub_a(RP) >= sub_b(RD) ?
1105 ; set the carry flag if sub_b(xRD) is equal to or greater than sub_a(xRP):
1106 movff xRD+0,sub_b+0
1107 movff xRD+1,sub_b+1
1108 call subU16 ; sub_c = sub_a - sub_b
1109 btfsc neg_flag ; >=0?
1110 return ; No
1111 ; store the RO=RP-RD for drawing
1112 movff sub_c+0,xC+0
1113 movff sub_c+1,xC+1
1114
1115 ; 1/b. check if it's viewable ? sub_a(RP)+up(width) < sub_b(RD)+160
1116 ; if already above, no need to process the rest of the labels
1117 ;movff up,WREG ; don't worry about the width, low level call prevents overload
1118 movlw d'2' ; .. but still avoid thin mess on the side of the display
1119 addwf sub_a+0,1
1120 btfsc STATUS, C
1121 incf sub_a+1
1122
1123 movff xRDr+0,sub_b+0
1124 movff xRDr+1,sub_b+1
1125 call subU16 ; sub_c = sub_a - sub_b
1126 btfss neg_flag ; ? <0
1127 bra TFT_dive_compass_label_end ; No
1128
1129 ; 2. restore RO=RP-RD from 1/a.
1130 movff xC+0,lo
1131
1132 ; 3. Clear the segment from DD(hi) to lo
1133 ; don't do a clear if we are at 0 (zero) otherwise it will blink
1134 ; ?because of the width underflow?
1135 movlw d'0'
1136 cpfsgt lo
1137 bra TFT_dive_compass_label_proc_p
1138 rcall TFT_dive_compass_clr_label
1139 TFT_dive_compass_label_proc_p:
1140 ; 4. print the SQ on the screen
1141 call TFT_standard_color
1142 bsf print_compass_label
1143 rcall TFT_dive_compass_label_print
1144 ; 6. retain the new display positions
1145 movff hi,divB ; old-hi will be used by the c_mk : clear+marker printing
1146 movff lo,hi
1147 movff up,WREG
1148 addwf hi,1
1149 movff lo,xLO
1150 movff hi,xHI
1151 return
1152
1153 TFT_dive_compass_label_print:
1154 movlw dive_compass_label_row
1155 movff WREG,win_top
1156 movff lo,win_leftx2
1157 movlw FT_SMALL
1158 movff WREG,win_font
1159 return
1160
1161 TFT_dive_compass_c_mk:
1162 ; Common task to draw center line and marker
1163 ; until a proper implementation make it simple:
1164 rcall TFT_dive_compass_mk
1165 rcall TFT_dive_compass_cline
1166 return
1167
1168 TFT_dive_compass_mk:
1169 ; draw the bearing on the screen if visible and if we just put something over it
1170 btfss compass_bearing_set
1171 return ; bearing_set=0 nothing to display
1172
1173 btfss compass_bearing_vis
1174 return ; bearing set but not visible
1175
1176 ; save lo/hi from trashing
1177 movff lo,xA+0
1178 movff hi,xA+1
1179
1180 ; did we just update the marker's position?
1181 ; DD.......DD
1182 ; CM+2>=DD(old) or CM-2<=DD
1183 ; ToDo
1184
1185 btfss compass_bearing_ahd
1186 bra TFT_dive_compass_mk_rear
1187 ;TFT_dive_compass_mk_front:
1188 clrf lo
1189 movff xCM,lo
1190 bsf print_compass_label ; set=green marker
1191 rcall TFT_dive_compass_mk_print
1192 bcf print_compass_label
1193 bra TFT_dive_compass_mk_end
1194
1195 TFT_dive_compass_mk_rear:
1196 clrf lo
1197 movff xCM,lo
1198 bcf print_compass_label ; set=red marker
1199 rcall TFT_dive_compass_mk_print
1200
1201 TFT_dive_compass_mk_end:
1202 movff xA+0,lo
1203 movff xA+1,hi
1204 return
1205
1206 TFT_dive_compass_mk_print:
1207 movlw d'1'
1208 cpfsgt lo
1209 bra TFT_dive_compass_mk_print_2 ; lo<1, skip the first line
1210 movlw d'2'
1211 subwf lo,0
1212 movff WREG,win_leftx2
1213 rcall TFT_dive_compass_mk_print_3
1214 TFT_dive_compass_mk_print_2:
1215 movlw d'2'
1216 addwf lo,0
1217 movff WREG,win_leftx2
1218 rcall TFT_dive_compass_mk_print_3
1219 return
1220 TFT_dive_compass_mk_print_3:
1221 movlw dive_compass_label_row
1222 movff WREG,win_top
1223 movlw dive_compass_label_height-.2
1224 movff WREG,win_height
1225 movlw d'2'
1226 movff WREG,win_width
1227 movlw d'2'
1228 movff WREG,win_bargraph
1229 movlw color_green
1230 btfss print_compass_label
1231 movlw color_red
1232 call TFT_set_color
1233 call TFT_box
1234 return
1235
1236 TFT_dive_compass_clr_label:
1237 movlw dive_compass_label_row-.2 ; set top & height
1238 movff WREG,win_top
1239 movlw dive_compass_label_height+.2
1240 movff WREG,win_height
1241 rcall TFT_dive_compass_clear
1242 return
1243
1244 TFT_dive_compass_clr_ruler:
1245 ; top tick
1246 movlw dive_compass_tick_top_top ; set top & height
1247 movff WREG,win_top
1248 movlw dive_compass_tick_height
1249 movff WREG,win_height
1250 rcall TFT_dive_compass_clear
1251 ;bottom tick
1252 movlw dive_compass_tick_bot_top ; set top & height
1253 movff WREG,win_top
1254 movlw dive_compass_tick_height
1255 movff WREG,win_height
1256 rcall TFT_dive_compass_clear
1257 return
1258
1259 TFT_dive_compass_clear:
1260 ; we receive RM in lo and DD in hi
1261 ; calculate width = RM-D
1262 movff hi,WREG
1263 subwf lo,0
1264 movff WREG,win_width ; RM-DD
1265 movff WREG,win_bargraph
1266 movff hi,win_leftx2
1267 movlw color_black
1268 call TFT_set_color
1269 call TFT_box
1270 return
1271
1272 TFT_dive_compass_ruler_print:
1273 ; we receive RM in lo and DD in hi
1274 movlw dive_compass_tick_top_top
1275 movff WREG,win_top
1276 movlw dive_compass_tick_height
1277 movff WREG,win_height
1278 movlw d'2'
1279 movff WREG,win_width
1280 movlw d'2'
1281 movff WREG,win_bargraph
1282 movff lo,win_leftx2 ; 0..159
1283 call TFT_standard_color
1284 call TFT_box
1285 movlw dive_compass_tick_bot_top
1286 movff WREG,win_top
1287 movlw dive_compass_tick_height
1288 movff WREG,win_height
1289 call TFT_standard_color
1290 call TFT_box
1291 return
1292
1293 TFT_dive_compass_cline:
1294 movlw color_yellow
1295 WIN_BOX_COLOR dive_compass_tick_top_top,dive_compass_tick_bot_bot,.80,.81
1296 return
1297
1298 tft_compass_cardinal:
1299 btfsc hi,0 ; Heading >255°?
1300 bra tft_compass_cardinal2 ; Yes must be W, NW or N
1301 ; No, Must be W, SW, S, SE, E, NE or N
1302 movlw .23
1303 subwf lo,W
1304 btfss STATUS,C
1305 bra tft_compass_cardinal_N
1306 movlw .68
1307 subwf lo,W
1308 btfss STATUS,C
1309 bra tft_compass_cardinal_NE
1310 movlw .113
1311 subwf lo,W
1312 btfss STATUS,C
1313 bra tft_compass_cardinal_E
1314 movlw .158
1315 subwf lo,W
1316 btfss STATUS,C
1317 bra tft_compass_cardinal_SE
1318 movlw .203
1319 subwf lo,W
1320 btfss STATUS,C
1321 bra tft_compass_cardinal_S
1322 movlw .248
1323 subwf lo,W
1324 btfss STATUS,C
1325 bra tft_compass_cardinal_SW
1326 bra tft_compass_cardinal_W
1327
1328 tft_compass_cardinal2:
1329 movlw .37
1330 subwf lo,W
1331 btfss STATUS,C
1332 bra tft_compass_cardinal_W
1333 movlw .82
1334 subwf lo,W
1335 btfss STATUS,C
1336 bra tft_compass_cardinal_NW
1337 ; bra tft_compass_cardinal_N
1338 tft_compass_cardinal_N:
1339 STRCAT_TEXT tN
1340 return
1341 tft_compass_cardinal_NE:
1342 STRCAT_TEXT tNE
1343 return
1344 tft_compass_cardinal_E:
1345 STRCAT_TEXT tE
1346 return
1347 tft_compass_cardinal_SE:
1348 STRCAT_TEXT tSE
1349 return
1350 tft_compass_cardinal_S:
1351 STRCAT_TEXT tS
1352 return
1353 tft_compass_cardinal_SW:
1354 STRCAT_TEXT tSW
1355 return
1356 tft_compass_cardinal_W:
1357 STRCAT_TEXT tW
1358 return
1359 tft_compass_cardinal_NW:
1360 STRCAT_TEXT tNW
1361 return
1362
1363 compass_heading_common:
1364 call speed_normal
1365 movlw compass_averaging ; numbers of extra averaging
1366 movwf up
1367 compass_heading_common2:
1368 rcall TFT_get_compass
1369 decfsz up,F
1370 bra compass_heading_common2
1371 extern compass
1372 call compass ; Do compass corrections.
1373 banksel common
1374
1375 ; More then compass_fast_treshold?
1376 movff compass_heading_old+0,sub_a+0
1377 movff compass_heading_old+1,sub_a+1
1378 movff compass_heading+0,sub_b+0
1379 movff compass_heading+1,sub_b+1
1380 call sub16
1381 btfss neg_flag ; <0?
1382 bra compass_heading_common3 ; No, test for threshold
1383 ; Yes, subtract the other way round
1384 movff compass_heading+0,sub_a+0
1385 movff compass_heading+1,sub_a+1
1386 movff compass_heading_old+0,sub_b+0
1387 movff compass_heading_old+1,sub_b+1
1388 call sub16
1389 compass_heading_common3:
1390 movff compass_heading+0,compass_heading_old+0 ; copy new "old"
1391 movff compass_heading+1,compass_heading_old+1
1392
1393 bcf compass_fast_mode
1394 movlw compass_fast_treshold
1395 cpfslt sub_c+0 ; > compass_fast_treshold?
1396 bsf compass_fast_mode ; Yes!
1397
1398 btfss compass_fast_mode ; In fast mode?
1399 return ; No.
1400 ; Yes.
1401 movff compass_heading+0,lo
1402 movff compass_heading+1,hi
1403 call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required
1404 movff lo,compass_heading_shown+0
1405 movff hi,compass_heading_shown+1
1406 return
1407
1408 TFT_get_compass:
1409 call I2C_RX_compass ; Test Compass
1410 call I2C_RX_accelerometer ; Test Accelerometer
1411 call compass_filter ; Filter Raw compass + accel readings.
1412 banksel common
1413 return
1414
339 1415
340 END 1416 END