Rev 2195 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2195 | Rev 2199 | ||
|---|---|---|---|
| Line 584... | Line 584... | ||
| 584 | 584 | ||
| 585 | 585 | ||
| Line 586... | Line 586... | ||
| 586 | void Debug_GPS (void) |
586 | void Debug_GPS (void) |
| Line 587... | Line 587... | ||
| 587 | { |
587 | { |
| 588 | 588 | ||
| Line 589... | Line 589... | ||
| 589 | uint8_t redraw; |
589 | uint8_t redraw; |
| 590 | 590 | ||
| 591 | double l1; |
591 | double l1; |
| Line 592... | Line 592... | ||
| 592 | GPS_Pos_t currpos; |
592 | GPS_Pos_t currpos; |
| 593 | 593 | ||
| 594 | set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
594 | set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
| Line 595... | Line 595... | ||
| 595 | redraw = true; |
595 | redraw = true; |
| 596 | uint8_t mktimeout = false; |
596 | uint8_t mktimeout = false; |
| Line 597... | Line 597... | ||
| 597 | 597 | ||
| 598 | uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete |
598 | uint32_t NMEA_GPGGA_counter_old = 0; // Merker: zaehlt empfangene GPGGA-Pakete |
| 599 | uint32_t send_followme_counter; |
599 | uint32_t send_followme_counter; |
| 600 | Point_t FollowMe; |
600 | Point_t FollowMe; |
| 601 | 601 | ||
| 602 | 602 | ||
| 603 | while( true ) |
603 | while( true ) |
| 604 | { |
604 | { |
| 605 | 605 | ||
| 606 | //######## Quell Koordinaten ############################## |
606 | //######## Quell Koordinaten ############################## |
| 607 | //# |
607 | //# |
| Line 608... | Line 608... | ||
| 608 | // NMEApos.lat = 0x0FB51D1F; //# |
608 | // NMEApos.lat = 0x0FB51D1F; //# |
| - | 609 | // NMEApos.lon = 0xE2CF4105; //# |
|
| Line 609... | Line -... | ||
| 609 | // NMEApos.lon = 0xE2CF4105; //# |
- | |
| 610 | //# |
- | |
| 611 | // NMEApos.lat = 0x1f1db5fb; //# |
- | |
| 612 | // NMEApos.lon = 0x0541cfe2; //# |
- | |
| Line 613... | Line -... | ||
| 613 | - | ||
| 614 | // l1 = NMEApos.lat; |
- | |
| 615 | //# |
610 | //# |
| 616 | //######################################################### |
611 | NMEApos.lat = 0x1f1db5fb; //# |
| 617 | 612 | NMEApos.lon = 0x0541cfe2; //# |
|
| 618 | int ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
613 | |
| Line 619... | Line -... | ||
| 619 | - | ||
| 620 | if( ok <= 0 ) |
- | |
| 621 | { |
- | |
| 622 | return; // Fehler bzgl. BT GPS-Maus -> exit |
- | |
| 623 | } |
- | |
| 624 | - | ||
| 625 | if(NMEA_isdataready() && receiveNMEA) |
- | |
| 626 | { |
- | |
| 627 | if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
- | |
| 628 | { |
- | |
| 629 | if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
- | |
| 630 | { |
- | |
| 631 | - | ||
| 632 | NMEApos.lat = NMEA.Latitude; |
- | |
| 633 | NMEApos.lon = NMEA.Longitude; |
- | |
| 634 | //Config.FM_Refresh |
- | |
| 635 | /* |
- | |
| 636 | FollowMe.Position.Status = NEWDATA; |
- | |
| 637 | FollowMe.Position.Longitude = NMEA.Longitude; |
614 | // l1 = NMEApos.lat; |
| 638 | FollowMe.Position.Latitude = NMEA.Latitude; |
- | |
| 639 | FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
- | |
| 640 | // FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
- | |
| 641 | - | ||
| 642 | FollowMe.Heading = -1; // invalid heading |
- | |
| 643 | FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
- | |
| 644 | FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
- | |
| 645 | // FollowMe.Event_Flag = 0; // no event |
- | |
| 646 | FollowMe.Event_Flag = 1; // 20.7.2015 CB |
- | |
| 647 | FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
- | |
| 648 | FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
- | |
| 649 | FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
- | |
| 650 | FollowMe.Name[1] = 'O'; |
- | |
| 651 | FollowMe.Name[2] = 'L'; |
- | |
| 652 | FollowMe.Name[3] = 'L'; |
- | |
| 653 | // FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
- | |
| 654 | FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
- | |
| 655 | FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
- | |
| Line 656... | Line 615... | ||
| 656 | FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
615 | //# |
| 657 | FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
616 | //######################################################### |
| - | 617 | ||
| - | 618 | /* int ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
|
| - | 619 | //lcd_cls (); |
|
| - | 620 | ||
| Line -... | Line 621... | ||
| - | 621 | ||
| 658 | FollowMe.reserve[0] = 0; // reserve |
622 | if( ok <= 0 ) |
| 659 | FollowMe.reserve[1] = 0; // reserve |
623 | { |
| - | 624 | return; // Fehler bzgl. BT GPS-Maus -> exit |
|
| - | 625 | } |
|
| - | 626 | ||
| 660 | 627 | receiveNMEA = true; |
|
| - | 628 | ||
| - | 629 | if(NMEA_isdataready() && receiveNMEA) |
|
| - | 630 | { |
|
| - | 631 | if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
|
| - | 632 | { |
|
| - | 633 | if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
|
| - | 634 | { |
|
| - | 635 | ||
| - | 636 | NMEApos.lat = NMEA.Latitude; |
|
| - | 637 | NMEApos.lon = NMEA.Longitude; |
|
| - | 638 | //Config.FM_Refresh |
|
| - | 639 | /* |
|
| - | 640 | FollowMe.Position.Status = NEWDATA; |
|
| 661 | SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
641 | FollowMe.Position.Longitude = NMEA.Longitude; |
| - | 642 | FollowMe.Position.Latitude = NMEA.Latitude; |
|
| - | 643 | FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
|
| - | 644 | // FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
|
| - | 645 | ||
| - | 646 | FollowMe.Heading = -1; // invalid heading |
|
| - | 647 | FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
|
| - | 648 | FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
|
| 662 | send_followme_counter++; |
649 | // FollowMe.Event_Flag = 0; // no event |
| - | 650 | FollowMe.Event_Flag = 1; // 20.7.2015 CB |
|
| 663 | 651 | FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
|
| - | 652 | FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
|
| - | 653 | FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
|
| - | 654 | FollowMe.Name[1] = 'O'; |
|
| - | 655 | FollowMe.Name[2] = 'L'; |
|
| Line 664... | Line -... | ||
| 664 | //void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
- | |
| 665 | // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
- | |
| 666 | //SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
- | |
| - | 656 | FollowMe.Name[3] = 'L'; |
|
| Line -... | Line 657... | ||
| - | 657 | // FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
|
| - | 658 | FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
|
| - | 659 | FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
|
| - | 660 | FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
|
| - | 661 | FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
|
| - | 662 | FollowMe.reserve[0] = 0; // reserve |
|
| - | 663 | FollowMe.reserve[1] = 0; // reserve |
|
| - | 664 | ||
| - | 665 | SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
|
| Line -... | Line 666... | ||
| - | 666 | send_followme_counter++; |
|
| - | 667 | ||
| - | 668 | //void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
|
| - | 669 | // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
|
| - | 670 | //SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
|
| 667 | */ |
671 | |
| 668 | } |
672 | } |
| 669 | 673 | ||
| 670 | NMEA_GPGGA_counter_old = NMEA.Counter; |
674 | NMEA_GPGGA_counter_old = NMEA.Counter; |
| 671 | } |
675 | } |
| 672 | 676 | */ |
|
| 673 | //----------------------------------------- |
677 | //----------------------------------------- |
| 674 | // Screen redraw |
678 | // Screen redraw |
| 675 | //----------------------------------------- |
679 | //----------------------------------------- |
| Line 676... | Line 680... | ||
| 676 | if( redraw ) |
680 | if( redraw ) |
| 677 | { |
681 | { |
| Line 678... | Line 682... | ||
| 678 | lcd_cls(); |
682 | lcd_cls(); |
| 679 | 683 | ||
| 680 | lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("GPS Test") ); // Titel: oben, mitte |
684 | lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("GPS Test") ); // Titel: oben, mitte |
| Line 681... | Line 685... | ||
| 681 | lcdx_printf_center_P( 4, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
685 | lcdx_printf_center_P( 4, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
| Line 682... | Line 686... | ||
| 682 | lcdx_printf_center_P( 6, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
686 | lcdx_printf_center_P( 6, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
| 683 | 687 | ||
| 684 | 688 | ||
| 685 | // sprintf(printbuff, "\r\nLat_Source: %ld", NMEA.Latitude); |
689 | // sprintf(printbuff, "\r\nLat_Source: %ld", NMEA.Latitude); |
| Line 686... | Line 690... | ||
| 686 | // uart1_puts(printbuff); |
690 | // uart1_puts(printbuff); |
| 687 | // |
691 | // |
| 688 | // sprintf(printbuff, "\r\nLon_Source: %ld", NMEA.Longitude); |
692 | // sprintf(printbuff, "\r\nLon_Source: %ld", NMEA.Longitude); |
| 689 | // uart1_puts(printbuff); |
693 | // uart1_puts(printbuff); |
| 690 | // |
694 | // |
| Line 691... | Line 695... | ||
| 691 | // |
695 | // |
| Line 692... | Line 696... | ||
| 692 | // writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
696 | // writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
| Line 693... | Line 697... | ||
| 693 | // writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); |
697 | // writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); |
| Line 694... | Line 698... | ||
| 694 | 698 | ||
| 695 | redraw = false; |
699 | redraw = false; |
| Line 696... | Line 700... | ||
| 696 | } |
700 | } |
| Line 697... | Line 701... | ||
| 697 | 701 | ||
| 698 | //################# |
702 | //################# |
| 699 | //# DISTANCE TO TARGET |
703 | //# DISTANCE TO TARGET |
| 700 | //################# |
704 | //################# |
| 701 | 705 | ||
| 702 | lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance ); |
706 | lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance ); |
| 703 | 707 | ||
| Line 704... | Line 708... | ||
| 704 | //################# |
708 | //################# |
| 705 | //# TARGET Azimuth |
709 | //# TARGET Azimuth |
| 706 | //################# |
710 | //################# |
| 707 | lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth); |
711 | lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth); |
| 708 | 712 | ||
| 709 | 713 | ||
| 710 | - | ||
| 711 | 714 | ||
| Line -... | Line 715... | ||
| - | 715 | ||
| 712 | //----------------- |
716 | //----------------- |
| Line 713... | Line 717... | ||
| 713 | // Original: Lat / Long |
717 | // Original: Lat / Long |