Mercurial > public > hwos_code
comparison src/compass_ops.asm @ 258:da9074180bff
patch for bearing issue
author | heinrichsweikamp |
---|---|
date | Mon, 23 Mar 2015 19:41:27 +0100 (2015-03-23) |
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 |