Subversion Repositories Projects

/*
 * tracking.c
 *
 *  Created on: 13.02.2012
 *      Author: cebra
 */

#include "../cpu.h"
#include <string.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include <math.h>
#include "../main.h"
#include "../tracking/tracking.h"
#include "../tracking/ng_servo.h"
#include <avr/pgmspace.h>
#include "../bluetooth/fifo.h"
#include "../bluetooth/bluetooth.h"
#include "../lcd/lcd.h"

#include "../mk-data-structs.h"
#include "tools.h"
#include "../messages.h"
#include "../lcd/lcd.h"
#include "../eeprom/eeprom.h"
#include "../timer/timer.h"
#include "../uart/uart1.h"
#include "../uart/usart.h"
#include "../osd/osd.h"
#include "../tracking/mymath.h"
#include "../setup/setup.h"




#ifdef HWVERSION3_9

//#define MAX_POWER                               10
//#define getPower(x)                     (int32_t)pgm_read_dword(&powers[x])
//const int32_t PROGMEM           powers[MAX_POWER] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};
#define DLEFT       0
#define DRIGHT      1
#define DEG_TO_RAD  0.0174533      // degrees to radians = PI      / 180
#define RAD_TO_DEG  57.2957795      // radians to degrees = 180 / PI
#define AltFaktor   22.5
#define TIMEOUT 200     // 2 sec

NaviData_t *naviData;

mk_param_struct_t *mk_param_struct;

HomePos_t       MK_pos;                         // Home position of station
GPS_Pos_t       currentPos;                   // Current position of flying object
uint8_t  NMEAsatsInUse;                          // Number of satelites currently in use BT-Mouse
//uint8_t  MKsatsInUse;                            // Number of satelites currently in use Mikrokopter
int32_t NMEAlatitude, NMEAlongitude;            // Longitude, Latitude
int16_t NMEAaltitude;                           // Höhe in Meter
uint8_t posfix;                              // GPS Fix, 0 = Fix not available or invalid,1 = GPS SPS Mode, fix valid,
                                                // 2 = Differential GPS, SPS Mode, fix valid, 6 = Dead Reckoning Mode, fix valid
int16_t HDOP;                                // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser


uint8_t tracking = TRACKING_MIN;
uint8_t track_hyst = TRACKING_HYSTERESE;
uint8_t track_tx =0;
uint8_t coldstart;                              // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix
geo_t   geo;
int16_t     anglePan, angleTilt;


char  NMEAtime[9] = "GP:Ti:me";
//char NMEADate [6];

//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
//// zur aktuellen Position(nach Motorstart)
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos)
//{ double lat1, lon1, lat2, lon2, d1, dlat;
//        geo_t geo;
//
//        lon1 = MK_pos.Home_Lon;
//        lat1 = MK_pos.Home_Lat;
//        lon2 = (double)pos->Longitude   / 10000000.0;
//        lat2 = (double)pos->Latitude    / 10000000.0;
//
//        // Formel verwendet von http://www.kompf.de/gps/distcalc.html
//        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator
//        // es wird jedoch in Meter weiter gerechnet
//        d1       = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);
//        dlat = 111300 * (double)(lat1 - lat2);
//        // returns a value in metres http://www.kompf.de/gps/distcalc.html
//        geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit
//        if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400
//        geo.distance = sqrt(d1 * d1 + dlat * dlat);
//        return(geo);
//}

// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
// zur aktuellen Position(nach Motorstart)
geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos)
{ int32_t lat1, lon1, lat2, lon2;
        int32_t d1, dlat;
        geo_t geo;

        lon1 = home->Home_Lon;
        lat1 = home->Home_Lat;
        lon2 = pos->Longitude;
        lat2 = pos->Latitude;
        if (!CheckGPS)
          {
            lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0);
            lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0);
            lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0);
            lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0);
          }
//        lcd_printp_at (0, 3, PSTR("H"), 0);
//        lcd_printp_at (0, 4, PSTR("M"), 0);

        // Formel verwendet von http://www.kompf.de/gps/distcalc.html
        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator
        // es wird jedoch in dm Meter weiter gerechnet
        // (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1
        d1       = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000;
        dlat = 1113 * (lat1 - lat2) / 10000;
        geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit
        geo.distance = sqrt32(d1 * d1 + dlat * dlat);
        if (!CheckGPS)
          {
            lcd_printp_at (0, 5, PSTR("Bear:"), 0);
            lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0);
            lcd_printp_at (8, 5, PSTR("\x1e"), 0);
            lcd_printp_at (9, 5, PSTR("Dist:"), 0);
            lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0);
            lcd_printp_at (20, 5, PSTR("m"), 0);
          }


        return(geo);
}


void do_tracking(void)
{       static uint8_t hysteresis = 0;

    geo = calc_geo(&MK_pos, &currentPos);
    angleTilt = my_atan2((currentPos.Altitude - MK_pos.Home_Alt) / 100, geo.distance);
    if (geo.distance < 40 || (geo.distance < 60 && hysteresis)) {                   // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt
            geo.bearing = MK_pos.direction;
            if (currentPos.Altitude - MK_pos.Home_Alt < 4000) angleTilt = 0;        // man fliegt nicht direkt �ber Kopf
            hysteresis = 1;
    }
    else {
            hysteresis = 0;
    }

    // egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen
    // 360 grd negative Winkelwerte als positive
    anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90

    if (angleTilt < 0) angleTilt = 0;
    if (angleTilt > 180) angleTilt = 180;

    if (anglePan >= 180) {                          // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo
            anglePan = anglePan - 180;
            angleTilt = 180 - angleTilt;
    }

    servoSetAngle(0, anglePan);
    servoSetAngle(1, angleTilt);
        if (!CheckGPS)
          {
            lcd_printp_at (0, 6, PSTR("Pan :"), 0);
            write_ndigit_number_u (6, 6,  anglePan, 3, 1,0);
            lcd_printp_at (11, 6, PSTR("Tilt:"), 0);
            write_ndigit_number_u (17, 6,  angleTilt, 3, 1,0);
          }

//        write_ndigit_number_u (0, 5, (uint16_t)(currentPos.Altitude/10000000), 2, 0,0);
////        lcd_printp_at (4, 4, PSTR("."), 0);
//        write_ndigit_number_u (2, 5, (uint16_t)((currentPos.Altitude/1000) % 10000), 4, 1,0);
//        write_ndigit_number_u (6, 5, (uint16_t)((currentPos.Altitude/10) % 100), 2, 1,0);
//
//        write_ndigit_number_u (10, 5, (uint16_t)(MK_pos.Home_Alt/10000000), 2, 0,0);
////        lcd_printp_at (4, 4, PSTR("."), 0);
//        write_ndigit_number_u (12, 5, (uint16_t)((MK_pos.Home_Alt/1000) % 10000), 4, 1,0);
//        write_ndigit_number_u (16, 5, (uint16_t)((MK_pos.Home_Alt/10) % 100), 2, 1,0);


}
//*******************************************************************************************************
uint8_t PKT_trackingBT(void)  // Tracking mit NMEA-Daten von BT-Maus

{

  uint8_t BT_WhasOn = 0;
  uint8_t BT_status;
  uint8_t flag;
  uint8_t tmp_dat;
  coldstart =1;

  {
//        lcd_printp_at(0,1, PSTR("try NMEA data from:"), 0);
      lcd_puts_at (0, 1,Config.gps_UsedDevName, 0);
      set_BTOn();
      BT_WhasOn = true;
      if (Config.BTIsSlave==true)
        {
          bt_downlink_init();
        }
        lcd_printp_at (18, 1, PSTR(" ?? "), 0);
        BT_status = bt_connect(Config.gps_UsedMac);
        if (BT_status==true)
          {
           lcd_printp_at (18, 1, PSTR(" OK "), 0);
           receiveNMEA = true;
          }
        else lcd_printp_at (17, 1, PSTR("FAIL"), 2);

      if (receiveNMEA==true)
        {
          lcd_printp_at (0, 2, PSTR("S Latitude  Longitude"), 2);
          lcd_cls_line (0,1,20);
          lcd_printp_at (0, 3, PSTR("H"), 0);
          lcd_printp_at (0, 4, PSTR("M"), 0);
          bt_rx_ready = 0;

          SwitchToNC();
          mode = 'O';
          // disable debug...
          //      RS232_request_mk_data (0, 'd', 0);
          tmp_dat = 0;
          SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);

          // request OSD Data from NC every 100ms
          //      RS232_request_mk_data (1, 'o', 100);
          tmp_dat = 10;
//            OSD_active = true;              // benötigt für Navidata Ausgabe an SV2
          SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);

          flag = 0;
          timer = TIMEOUT;
          abo_timer = ABO_TIMEOUT;


          do
            {
//                            bt_rx_ready = 0;
               if (!bt_receiveNMEA())
                 break;
               if (rxd_buffer_locked)
               {
                 timer = TIMEOUT;
                 Decode64 ();
                 naviData = (NaviData_t *) pRxData;

//#ifdef DEBUG
//                        debug_pgm(PSTR("setup Tracking_NMEA"));
//#endif
                 currentPos = naviData->CurrentPosition;
//                   currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;

//                   uint32_t lat = currentPos.Latitude;
//                   uint32_t lon = currentPos.Longitude;

//                   write_ndigit_number_u (2, 4, (uint16_t)(lat/10000000), 2, 0,0);
//                   lcd_printp_at (4, 4, PSTR("."), 0);
//                   write_ndigit_number_u (5, 4, (uint16_t)((lat/1000) % 10000), 4, 1,0);
//                   write_ndigit_number_u (9, 4, (uint16_t)((lat/10) % 100), 2, 1,0);
//

//                   write_ndigit_number_u (12, 4, (uint16_t)(lon/10000000), 2, 0,0);
//                   lcd_printp_at (14, 4, PSTR("."), 0);
//                   write_ndigit_number_u (15, 4, (uint16_t)((lon/1000) % 10000), 4, 1,0);
//                   write_ndigit_number_u (19, 4, (uint16_t)((lon/10) % 100),2, 1,0);

                  Tracking_NMEA();


//                    write_ndigit_number_u (2, 3, (uint16_t)(NMEAlatitude/10000000), 2, 0,0);
//                    lcd_printp_at (4, 3, PSTR("."), 0);
//                    write_ndigit_number_u (5, 3, (uint16_t)((NMEAlatitude/1000) % 10000), 4, 1,0);
//                    write_ndigit_number_u (9, 3, (uint16_t)((NMEAlatitude/10) % 100), 2, 1,0);
//
//
//                    write_ndigit_number_u (12, 3, (uint16_t)(NMEAlongitude/10000000), 2, 0,0);
//                    lcd_printp_at (14, 3, PSTR("."), 0);
//                    write_ndigit_number_u (15, 3, (uint16_t)((NMEAlongitude/1000) % 10000), 4, 1,0);
//                    write_ndigit_number_u (19, 3, (uint16_t)((NMEAlongitude/10) % 100), 2, 1,0);

  //                lcd_printp_at (0, 2, PSTR("GPS Time: "), 0);
                  if (!CheckGPS)
                    {
                      lcd_puts_at (13, 0, NMEAtime, 2);
                      lcd_printp_at (16, 1, PSTR("Sat:"), 0);
                      write_ndigit_number_u (19, 1,  NMEAsatsInUse, 2, 1,0);
                      lcd_printp_at (0, 1, PSTR("Fix:"), 0);
                      write_ndigit_number_u (4, 1,  posfix, 1, 1,0);
                      lcd_printp_at (6, 1, PSTR("HDOP:"), 0);
                      write_ndigit_number_u_10th (11, 1,  HDOP, 3, 0,0);
                    }


                  rxd_buffer_locked = FALSE;

                  if (!abo_timer)
                      {       // renew abo every 3 sec
                              // request OSD Data from NC every 100ms
                              //      RS232_request_mk_data (1, 'o', 100);
                              tmp_dat = 10;
                              SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);

                              abo_timer = ABO_TIMEOUT;
                      }
                }//if (rxd_buffer_locked)

               if (!timer)
               {
                       OSD_Timeout(flag);
                       flag = 0;
                       error = 1;

               }


             } //end do


          while (!get_key_press (1 << KEY_ENTER) || !receiveNMEA==true || error ==1);
//                        while (!get_key_press (1 << KEY_ENTER));

          lcd_cls_line(0,1,21);
          lcd_cls_line(0,2,21);
          lcd_cls_line(0,3,21);
          lcd_cls_line(0,4,21);
          lcd_cls_line(0,5,21);
          lcd_cls_line(0,6,21);
          if (!receiveNMEA) lcd_printp_at (0, 2, PSTR("lost BT data"), 0);
//            if (error ==1) lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0);
          lcd_printp_at (0, 3, PSTR("GPS trennen"), 0);
        }
      else
        {
          lcd_printp_at (0, 4, PSTR("Error at connecting"), 0);
          lcd_printp_at (0, 5, PSTR("switch on BT Mouse!!"), 0);
          while (!get_key_press (1 << KEY_ENTER));
        }
      receiveNMEA = false;
      if  (!bt_disconnect()) lcd_printp_at (0, 3, PSTR("Fehler beim Trennen"), 0);

      set_BTOff();
      return true;
  }
}

//*******************************************************************************************************

uint8_t PKT_trackingMK(void)  // Tracking mit GPS-Daten vom Mikrokopter

{

//  uint8_t BT_WhasOn = 0;
//  uint8_t BT_status;
  uint8_t GPSfix=0;
  uint8_t tmp_dat;
  uint8_t toggletimer=0;
  coldstart = true;


     lcd_printp_at (0, 2, PSTR("S Latitude  Longitude"), 2);

     lcd_cls_line (0,1,20);
//     lcd_printp_at (0, 3, PSTR("H"), 0);
//     lcd_printp_at (0, 4, PSTR("M"), 0);

     SwitchToNC();
     mode = 'O';
     // disable debug...
     //      RS232_request_mk_data (0, 'd', 0);
     tmp_dat = 0;
     SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);

     // request OSD Data from NC every 100ms
     //      RS232_request_mk_data (1, 'o', 100);
     tmp_dat = 10;
     SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
     timer = TIMEOUT;
     abo_timer = ABO_TIMEOUT;
     error = 0;

     do
       {

          if (rxd_buffer_locked)
          {
            timer = TIMEOUT;
            Decode64 ();
            naviData = (NaviData_t *) pRxData;
            //OSD_Screen_Element (18, 1, OSD_SATS_IN_USE,1);
            //if (GPSfix == true)  OSD_Screen_Element (0, 1, OSD_STATUS_FLAGS,1);
            OSD_Element_SatsInUse( 18, 1, 1);
            if (GPSfix == true)  OSD_Element_StatusFlags( 0, 1);

            if (!(naviData->NCFlags & NC_FLAG_GPS_OK))
              {
                toggletimer++;
                if (toggletimer == 50) toggletimer = 0;
                if (toggletimer == 25) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 2);
                if (toggletimer == 1) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 0);

                rxd_buffer_locked = false;
                GPSfix = false;

              }
            else GPSfix = true;

            if (GPSfix)
              {
                if (coldstart)
                    {

                      // erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen
                      if (naviData->FCStatusFlags & FC_FLAG_MOTOR_START) {
                              MK_pos.Home_Lon = naviData->HomePosition.Longitude;
                              MK_pos.Home_Lat = naviData->HomePosition.Latitude;
                              MK_pos.Home_Alt = naviData->HomePosition.Altitude;
                              MK_pos.direction = naviData->CompassHeading;
                              coldstart = false;
                              rxd_buffer_locked = false;
                              lcd_printp_at(0,1, PSTR("                  "), 0);

                          }
                      else
                        {
                          lcd_printp_at(0,1, PSTR("GPS ok, start ok  "), 0);
                          rxd_buffer_locked = false;
                        }

                    }
                  else
                    {  //run


                        currentPos = naviData->CurrentPosition;
                        currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
                        do_tracking();
    //                           lcd_puts_at (13, 0, NMEAtime, 2);
    //                           lcd_printp_at (16, 1, PSTR("Sat:"), 0);
    //                           write_ndigit_number_u (19, 1,  NMEAsatsInUse, 2, 1,0);
    //                           lcd_printp_at (0, 1, PSTR("Fix:"), 0);
    //                           write_ndigit_number_u (4, 1,  posfix, 1, 1,0);
    //                           lcd_printp_at (6, 1, PSTR("HDOP:"), 0);
    //                           write_ndigit_number_u_10th (11, 1,  HDOP, 3, 0,0);
                         rxd_buffer_locked = FALSE;


                       } // run
                  }
            if (!abo_timer)
                {       // renew abo every 3 sec
                        // request OSD Data from NC every 100ms
                        //      RS232_request_mk_data (1, 'o', 100);
                        tmp_dat = 10;
                        SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);

                        abo_timer = ABO_TIMEOUT;
                }

          } //rx_buffer_locked

          if (!timer)
          {
                  OSD_Timeout(1);
                  error = 1;
          }



     } //end do


     while ((!get_key_press (1 << KEY_ENTER)) && (error ==0));


     lcd_cls_line(0,1,21);
     lcd_cls_line(0,2,21);
     lcd_cls_line(0,3,21);
     lcd_cls_line(0,4,21);
     lcd_cls_line(0,5,21);
     lcd_cls_line(0,6,21);

    if (error ==1)
      {
        lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0);
        _delay_ms(2000);
      }
    return true;
}

//*******************************************************************************************************

void PKT_tracking(void)
{



  get_key_press(KEY_ALL);
  lcd_cls ();
  if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1)  lcd_printp_at(0,0, PSTR("Tracking Bluetooth   "), 2);
  if (Config.gps_UsedGPSMouse==GPS_Mikrokopter)      lcd_printp_at(0,0, PSTR(" Tracking Mikrokopter"), 2);
  lcd_printp_at (16, 7, PSTR("Ende"), 0);


  if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT();
  if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK();
  get_key_press(KEY_ALL);

}





//// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit
//// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450
//int32_t floatStrToInt(const char *s, int32_t power1)
//{       char                            *endPtr;
//        int32_t                 v = strtol(s, &endPtr, 10);
//
//        if (*endPtr == '.') {
//                for (s = endPtr + 1; *s && power1; s++) {
//                        v = v * 10 + (*s - '0');
//                        --power1;
//                }
//        }
//        if (power1) {
//                // Table to avoid multiple multiplications
//                v = v * getPower(power1);
//        }
//        return v;
//}

// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps
int32_t         getLatitude(const char *s, const char *NS)
{       int32_t         deg = (s[0] - '0') * 10 + s[1] - '0';           // First 2 chars are full degrees
        int32_t         min = floatStrToInt(&s[2], 6) / 6;                              // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps

        deg = deg * 10000000 + min;
        if (*NS == 'S') deg = -deg;
        return deg;
}


// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps
int32_t         getLongitude(const char *s, const char *WE)
{       int32_t         deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0';               // First 3 chars are full degrees
        int32_t         min = floatStrToInt(&s[3], 6) / 6;                              // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps

        deg = deg * 10000000 + min;
        if (*WE == 'W') deg = -deg;
        return deg;
}

void getNMEATime( const char *s)
{
uint8_t sem = 0;
uint8_t i;

 for ( i=0;i < 6; i++ )
   {
     NMEAtime[sem++] =  s[i];
      if (i==1 || i==3) NMEAtime[sem++] = ':';

   }
  NMEAtime[sem] = '\0';
}




//$GPGGA,191410.000,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45
//           ^      ^           ^            ^ ^  ^   ^       ^
//           |      |           |            | |  |   |       |
//           |      |           |            | |  |   |       Höhe Geoid minus
//           |      |           |            | |  |   |       Höhe Ellipsoid (WGS84)
//           |      |           |            | |  |   |       in Metern (48.0,M)
//           |      |           |            | |  |   |
//           |      |           |            | |  |   Höhe über Meer (über Geoid)in Metern (351.5,M)
//           |      |           |            | |  |
//           |      |           |            | |  HDOP (horizontal dilution
//           |      |           |            | |  of precision) Genauigkeit
//           |      |           |            | |
//           |      |           |            | Anzahl der erfassten Satelliten
//           |      |           |            |
//           |      |           |            Qualität der Messung
//           |      |           |            (0 = ungültig)
//           |      |           |            (1 = GPS)
//           |      |           |            (2 = DGPS)
//           |      |           |            (6 = geschätzt nur NMEA-0183 2.3)
//           |      |           |
//           |      |           Längengrad
//           |      |
//           |      Breitengrad
//           |
//           Uhrzeit

void Tracking_NMEA(void)
{
char *token;



        if (decodeNMEA()) {
                token = strtok((char*)data_decode, ",");
                if (!strcmp(token, "GPGGA"))
                  {
                        // $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,,
                        // Time
                        getNMEATime(strtok(0, ".")); //Zeit

                        strtok(0, ","); // Skip Rest von der Zeit
                        // Latitude
                        NMEAlatitude = getLatitude(strtok(0, ","), strtok(0, ",")); //N
                        // Longitude
                        NMEAlongitude = getLongitude(strtok(0, ","), strtok(0, ","));//E
                        // Signal valid? (Position Fix Indicator)
                        posfix = atoi(strtok(0, ",")); // Qualität
                        // Satellites in use
                        NMEAsatsInUse = atoi(strtok(0, ",")); //Anzahl Sats
                        // Dilition, best = 0.0
                        HDOP = floatStrToInt(strtok(0, ","),1); //Dilution

//                      // Altitude
                        NMEAaltitude = floatStrToInt(strtok(0, ","), 1);
//                        currentPos.Altitude = altitude;
//                        currentPos.Latitude = latitude;
//                        currentPos.Longitude = longitude;

//TODO: erstmal test                        if ((coldstart) && (satsInUse > 5))     {
                                // First position after reboot (or change of mode) will be the home position (facing north)
                                MK_pos.Home_Lon = NMEAlongitude;
                                MK_pos.Home_Lat = NMEAlatitude;

                                MK_pos.Home_Alt = NMEAaltitude;
                                MK_pos.direction = 0;
//                                coldstart = 0;
//                                        Double_Beep(DBEEPNMEAFIX, DBEEPMEAFIXP);

                                do_tracking();
//                        }
                }
        }
//        Displ_GPS();            // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar
}


uint8_t hexDigitToInt(uint8_t digit)
{
        if (digit >= '0' && digit <= '9') return digit - '0';
        if (digit >= 'a' && digit <= 'f') return digit - 'a' + 10;
        if (digit >= 'A' && digit <= 'F') return digit - 'A' + 10;
        return 0;
}


uint8_t decodeNMEA(void)
{
uint8_t  ret = 0;
uint8_t  crc;
uint8_t  tmpCRC = 0;
uint8_t  i;

        if (bt_rx_ready == 1 && bt_rx_len > 0) {
                // Calculate checksum
                for (i = 1; i < bt_rx_len && bt_rx_buffer[i] != '*'; i++) {
                        tmpCRC ^= bt_rx_buffer[i];
                }
                if (bt_rx_len >= i + 3) {
                        crc = hexDigitToInt(bt_rx_buffer[i + 1]) << 4 | hexDigitToInt(bt_rx_buffer[i + 2]);
                        if (crc == tmpCRC) {
                                bt_rx_buffer[i] = 0;
                                strcpy(data_decode, &bt_rx_buffer[1]);     // Data without $, crc
                                ret = 1;
 //                               wi232RX = 1;                                                                                                    // So antenna-symbol will blink
//                                cli();
//                                rx_timeout = 0;                                                                                         // Got valid data, reset counter
//                                sei();
                        }
                }
        }
//        if (rx_timeout < RX_TIME_OLD) wi232RX = 1;
        bt_rx_ready = 0;                                                                                                                           // Unlock buffer, next NMEA string can be received
        return ret;
}
#endif