Rev 2203 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2203 | Rev 2204 | ||
---|---|---|---|
Line 147... | Line 147... | ||
147 | uint16_t heading_home; |
147 | uint16_t heading_home; |
Line 148... | Line 148... | ||
148 | 148 | ||
149 | // Hier Höhenanzeigefehler Korrigieren |
149 | // Hier Höhenanzeigefehler Korrigieren |
Line -... | Line 150... | ||
- | 150 | #define AltimeterAdjust 1.5 |
|
- | 151 | ||
Line 150... | Line 152... | ||
150 | #define AltimeterAdjust 1.5 |
152 | positionOffset followMeOffset; |
151 | 153 | ||
152 | 154 | ||
Line 211... | Line 213... | ||
211 | { |
213 | { |
212 | return; // Fehler bzgl. BT GPS-Maus -> exit |
214 | return; // Fehler bzgl. BT GPS-Maus -> exit |
213 | } |
215 | } |
Line 214... | Line -... | ||
214 | - | ||
215 | 216 | ||
216 | 217 | ||
217 | //--------------------- |
218 | //--------------------- |
218 | // 2. Follow Me |
219 | // 2. Follow Me |
Line 413... | Line 414... | ||
413 | // Target: Lat / Long |
414 | // Target: Lat / Long |
414 | //----------------- |
415 | //----------------- |
415 | writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude |
416 | writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude |
416 | writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude |
417 | writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude |
Line 417... | Line -... | ||
417 | - | ||
418 | 418 | ||
Line 419... | Line 419... | ||
419 | rxd_buffer_locked = FALSE; |
419 | rxd_buffer_locked = FALSE; |
420 | 420 | ||
Line 430... | Line 430... | ||
430 | // { |
430 | // { |
431 | // NMEA_GetNewData(); |
431 | // NMEA_GetNewData(); |
432 | // } |
432 | // } |
Line 433... | Line -... | ||
433 | - | ||
434 | 433 | ||
435 | 434 | ||
436 | // Sourcecode aus NaviCtrl/V2.10e/uart1.c |
435 | // Sourcecode aus NaviCtrl/V2.10e/uart1.c |
437 | // FOLLOW_ME |
436 | // FOLLOW_ME |
438 | // else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
437 | // else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
Line 468... | Line 467... | ||
468 | // } |
467 | // } |
469 | // UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time |
468 | // UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time |
470 | // } |
469 | // } |
471 | // |
470 | // |
Line -... | Line 471... | ||
- | 471 | ||
Line -... | Line 472... | ||
- | 472 | #ifdef USE_FOLLOWME_STEP2 |
|
Line 472... | Line 473... | ||
472 | 473 | ||
Line 473... | Line 474... | ||
473 | 474 | sendFollowMeData(&FollowMe, &send_followme_counter, &NMEA_GPGGA_counter_old); |
|
474 | 475 | ||
475 | 476 | #else |
|
476 | 477 | ||
Line 521... | Line 522... | ||
521 | 522 | ||
522 | NMEA_GPGGA_counter_old = NMEA.Counter; |
523 | NMEA_GPGGA_counter_old = NMEA.Counter; |
523 | } |
524 | } |
Line -... | Line 525... | ||
- | 525 | } |
|
- | 526 | ||
524 | } |
527 | #endif |
525 | 528 | ||
526 | //----------------------------------------- |
529 | //----------------------------------------- |
527 | // TASTEN |
530 | // TASTEN |
528 | //----------------------------------------- |
531 | //----------------------------------------- |
Line 534... | Line 537... | ||
534 | if( get_key_press(1 << KEY_ENTER) ) |
537 | if( get_key_press(1 << KEY_ENTER) ) |
535 | { |
538 | { |
536 | break; |
539 | break; |
537 | } |
540 | } |
Line 538... | Line -... | ||
538 | - | ||
539 | 541 | ||
540 | //----------------------------------------- |
542 | //----------------------------------------- |
541 | //----------------------------------------- |
543 | //----------------------------------------- |
542 | if( !abo_timer ) |
544 | if( !abo_timer ) |
543 | { |
545 | { |
Line 548... | Line 550... | ||
548 | SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
550 | SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
Line 549... | Line 551... | ||
549 | 551 | ||
550 | abo_timer = ABO_TIMEOUT; |
552 | abo_timer = ABO_TIMEOUT; |
Line 551... | Line -... | ||
551 | } |
- | |
552 | - | ||
553 | 553 | } |
|
554 | 554 | ||
555 | //-------------------------- |
555 | //-------------------------- |
556 | // Daten Timeout vom MK? |
556 | // Daten Timeout vom MK? |
557 | //-------------------------- |
557 | //-------------------------- |
Line 565... | Line 565... | ||
565 | // OSD_MK_Connect( MK_CONNECT ); |
565 | // OSD_MK_Connect( MK_CONNECT ); |
566 | } |
566 | } |
Line 567... | Line 567... | ||
567 | 567 | ||
Line -... | Line 568... | ||
- | 568 | } // end: while( (receiveNMEA) ); |
|
Line -... | Line 569... | ||
- | 569 | ||
- | 570 | OSD_active = false; |
|
Line 568... | Line -... | ||
568 | } // end: while( (receiveNMEA) ); |
- | |
569 | - | ||
570 | - | ||
571 | - | ||
Line -... | Line 571... | ||
- | 571 | ||
- | 572 | GPSMouse_Disconnect(); |
|
- | 573 | } |
|
- | 574 | ||
- | 575 | ||
- | 576 | ||
- | 577 | ||
- | 578 | // Funktion sendFollowMeData angepasst auf Zeiger |
|
- | 579 | // ausgelagert als Funktion, da auch von FollowMeStep2 verwendet wird |
|
- | 580 | // TODO: testen mit Simulator und Kopter :-) |
|
- | 581 | ||
- | 582 | void sendFollowMeData(Point_t *tFollowMe, uint32_t *tsend_followme_counter, uint32_t *tNMEA_GPGGA_counter_old) |
|
- | 583 | { |
|
- | 584 | Point_t FollowMe = *tFollowMe; |
|
- | 585 | ||
- | 586 | if(NMEA_isdataready() && receiveNMEA) |
|
- | 587 | { |
|
- | 588 | if( NMEA.Counter > *tNMEA_GPGGA_counter_old ) |
|
- | 589 | { |
|
- | 590 | if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
|
- | 591 | { |
|
- | 592 | FollowMe.Position.Status = NEWDATA; |
|
- | 593 | FollowMe.Position.Longitude = NMEA.Longitude; |
|
- | 594 | FollowMe.Position.Latitude = NMEA.Latitude; |
|
- | 595 | FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
|
- | 596 | ||
- | 597 | FollowMe.Heading = -1; // invalid heading |
|
572 | //--------------------- |
598 | FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
573 | // BEENDEN |
599 | FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
574 | //--------------------- |
600 | FollowMe.Event_Flag = 1; // 20.7.2015 CB |
- | 601 | FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
|
- | 602 | FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
|
- | 603 | FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
|
- | 604 | FollowMe.Name[1] = 'O'; |
|
- | 605 | FollowMe.Name[2] = 'L'; |
|
- | 606 | FollowMe.Name[3] = 'L'; |
|
- | 607 | FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
|
- | 608 | FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
|
575 | OSD_active = false; |
609 | FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
- | 610 | FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
|
- | 611 | FollowMe.reserve[0] = 0; // reserve |
|
- | 612 | FollowMe.reserve[1] = 0; // reserve |
|
- | 613 | ||
- | 614 | SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
|
576 | 615 | *tsend_followme_counter += 1; |
|
Line -... | Line 616... | ||
- | 616 | } |
|
Line 577... | Line -... | ||
577 | - | ||
578 | //--------------------- |
617 | |
579 | // GPS beenden |
618 | *tNMEA_GPGGA_counter_old = NMEA.Counter; |
580 | //--------------------- |
619 | } |
581 | GPSMouse_Disconnect(); |
620 | } |
Line 582... | Line 621... | ||
582 | } |
621 | } |
583 | 622 | ||
Line 584... | Line -... | ||
584 | - | ||
585 | 623 | ||
586 | 624 | ||
587 | 625 | ||
588 | // |
626 | // FollowMeStep2: |
589 | #ifdef USE_FOLLOWME_STEP2 |
627 | |
Line 590... | Line -... | ||
590 | void Debug_GPS (void) |
- | |
591 | { |
628 | #ifdef USE_FOLLOWME_STEP2 |
592 | uint8_t redraw; |
629 | void Debug_GPS (void) |
- | 630 | { |
|
- | 631 | uint8_t redraw; |
|
Line 593... | Line -... | ||
593 | - | ||
594 | set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
- | |
595 | redraw = true; |
- | |
596 | - | ||
597 | - | ||
598 | int retcode = GPSMouse_Connect(); // Abfrage der GPS-Daten zum testen Quick an Dirty ;-) |
- | |
599 | if( retcode <= 0 ) |
- | |
600 | { |
- | |
601 | return; |
- | |
602 | } |
- | |
603 | - | ||
604 | 632 | ||
605 | while( true ) |
633 | set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
606 | { |
634 | redraw = true; |
Line 607... | Line 635... | ||
607 | 635 | ||
Line 640... | Line 668... | ||
640 | 668 | ||
641 | 669 | ||
Line -... | Line 670... | ||
- | 670 | writex_gpspos( 1, 2, NMEApos.lat , MNORMAL,0,0 ); // GPS-Maus: Latitude |
|
- | 671 | writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
|
- | 672 | ||
- | 673 | //followMeOffset.offset_lat = 10000; |
|
642 | writex_gpspos( 1, 2, NMEApos.lat , MNORMAL,0,0 ); // GPS-Maus: Latitude |
674 | //followMeOffset.offset_long = 10000; |
Line 643... | Line 675... | ||
643 | writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
675 | |
644 | 676 | followme_calculate_offset(Config.FM_Distance, Config.FM_Azimuth, &followMeOffset); |
|
Line 664... | Line 696... | ||
664 | 696 | ||
Line 665... | Line 697... | ||
665 | 697 |