Mercurial > public > hwos_code

comparison src/compass_ops.asm @ 258:da9074180bff

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
patch for bearing issue
author heinrichsweikamp
date Mon, 23 Mar 2015 19:41:27 +0100
parents 5dd0f39d05d4
children 151148e2281f
comparison
equal deleted inserted replaced
257:5dd0f39d05d4 258:da9074180bff
538 movff WREG,sub_b+0 538 movff WREG,sub_b+0
539 call subU16 ; sub_c = sub_a - sub_b 539 call subU16 ; sub_c = sub_a - sub_b
540 movff sub_c+1,xRD180+1 540 movff sub_c+1,xRD180+1
541 movff sub_c+0,xRD180+0 541 movff sub_c+0,xRD180+0
542 542
543 TFT_dive_compass_bearing_1:
544 ; calculate bearing position and visibility (ahead or behind)
545 bcf compass_bearing_vis ; default is not-visibly
546 bcf compass_bearing_ahd ; default is behind
543 ; get the bearing virtual display offset, store it to divA 547 ; get the bearing virtual display offset, store it to divA
544 movff compass_bearing+0,xA+0 548 movff compass_bearing+0,xA+0
545 movff compass_bearing+1,xA+1 549 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') 550 movlw high(d'360')
558 addwf xA+1,1 551 addwf xA+1,1
559 movlw low(d'360') 552 movlw low(d'360')
560 addwf xA+0,1 553 addwf xA+0,1
561 btfsc STATUS,C 554 btfsc STATUS,C
562 incf xA+1 555 incf xA+1
563 ; save it for the direction (<< or >>) calculation 556 ; save it to reuse for upper/lower turns and ahead/behind checks
564 movff xA+1,divA+1 557 movff xA+1,divA+1
565 movff xA+0,divA+0 558 movff xA+0,divA+0
566 559
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 560 ; check if it's ahead
572 ; load the bearing offset into sub_a 561 ; load the bearing offset into sub_a
573 movff divA+1,sub_a+1 562 movff divA+1,sub_a+1
574 movff divA+0,sub_a+0 563 movff divA+0,sub_a+0
575 ; load the display offset back to sub_b 564 ; load the display offset back to sub_b
578 rcall TFT_dive_compass_bearing_ap 567 rcall TFT_dive_compass_bearing_ap
579 ;test if we found it 568 ;test if we found it
580 btfsc compass_bearing_vis 569 btfsc compass_bearing_vis
581 bra TFT_dive_compass_bearing_dir 570 bra TFT_dive_compass_bearing_dir
582 571
583 ; check if it's ahead with a furr turn 572 ; check if it's ahead with an upper turn
584 ; load the bearing offset into sub_a 573 ; load the bearing offset into sub_a
585 movff divA+1,sub_a+1 574 movff divA+1,sub_a+1
586 movff divA+0,sub_a+0 575 movff divA+0,sub_a+0
587 ; load the display offset back to sub_b 576 ; load the display offset back to sub_b
588 movff xRD+0,sub_b+0 577 movff xRD+0,sub_b+0
591 addwf sub_b+1,1 580 addwf sub_b+1,1
592 movlw low(d'360') 581 movlw low(d'360')
593 addwf sub_b+0,1 582 addwf sub_b+0,1
594 btfsc STATUS,C 583 btfsc STATUS,C
595 incf sub_b+1 584 incf sub_b+1
585 rcall TFT_dive_compass_bearing_ap
586 ;test if we found it
587 btfsc compass_bearing_vis
588 bra TFT_dive_compass_bearing_dir
589
590 ; check if it's ahead with a lower turn
591 ; load the bearing offset into sub_a
592 movff divA+1,sub_a+1
593 movff divA+0,sub_a+0
594 movlw high(d'360')
595 addwf sub_a+1,1
596 movlw low(d'360')
597 addwf sub_a+0,1
598 btfsc STATUS,C
599 incf sub_a+1
600 ; load the display offset back to sub_b
601 movff xRD+0,sub_b+0
602 movff xRD+1,sub_b+1
596 rcall TFT_dive_compass_bearing_ap 603 rcall TFT_dive_compass_bearing_ap
597 ;test if we found it 604 ;test if we found it
598 btfsc compass_bearing_vis 605 btfsc compass_bearing_vis
599 bra TFT_dive_compass_bearing_dir 606 bra TFT_dive_compass_bearing_dir
600 607
714 bra TFT_dive_compass_bearing_lr 721 bra TFT_dive_compass_bearing_lr
715 bsf compass_bearing_eq 722 bsf compass_bearing_eq
716 bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler 723 bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler
717 724
718 TFT_dive_compass_bearing_lr: 725 TFT_dive_compass_bearing_lr:
726 ; get the bearing virtual display offset
727 movff compass_bearing+0,xA+0
728 movff compass_bearing+1,xA+1
729 ; divA =IF (U10>292;U10;U10+360)
730 movlw high(d'292')
731 movff WREG,sub_a+1
732 movlw low(d'292')
733 movff WREG,sub_a+0
734 movff xA+1,sub_b+1
735 movff xA+0,sub_b+0
736 call subU16 ; sub_c = sub_a - sub_b
737 btfsc neg_flag ; xA>292
738 bra TFT_dive_compass_bearing_lr_1 ;yes
739 ; no, xA<=292
740 movlw high(d'360')
741 addwf xA+1,1
742 movlw low(d'360')
743 addwf xA+0,1
744 btfsc STATUS,C
745 incf xA+1
746 TFT_dive_compass_bearing_lr_1:
719 ; 1. calculate whether bearing is to left or to right 747 ; 1. calculate whether bearing is to left or to right
720 bsf compass_bearing_lft ; to the left by default 748 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 749 ; xC: save center value to compare the direction to front value
725 movff xA+1,xC+1 750 movff xA+1,xC+1
726 movff xA+0,xC+0 751 movff xA+0,xC+0
727 ; xB: we need the left side for comparism... left = -180 752 ; xB: we need the left side for comparism... left = -180
728 movff xA+1,sub_a+1 753 movff xA+1,sub_a+1