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