GPS

Philipp Elsner und Hendrik Böttcher
Praktikum von 07.08-03.09.12




{autotoc addNumbering=true|addNumberingToc=true}

Beschreibung der Tätigkeit

 

Philipp

Im Rahmen der Ingenieurpraxis habe ich einen Teil eines Navigationsrechners entwickelt und getestet. Meine Hauptaufgabe war die Entwicklung der Hardware. Dazu gehören eine Platine mit Schnittstellen, eine externe Stromversorgung, ein LCD-Display und ein externer Speicher. Ein Pegelwandler und alle anderen Komponenten wurden an den Arduino angeschlossen. Die Schnittstellenplatine beinhaltet die Anschlüsse für den GPS-Empfänger, Ausgänge für das NMEA-Protokoll, das TSIP-Protokoll, den Datenlogger und den Arduino. Das TSIP(Trimble Standard Interface Protokoll) wird mit Hilfe eines USB-RS232-Adapters auf den PC übertragen, damit die Daten mit dem Programm des Herstellers ausgewertet werden können. Das NMEA-Protokoll wird über die Serielle Schnittstelle des GPS-Empfängers an den Arduino übermittelt und dort verarbeitet. Die Programmierung der Software, welche das gesendete Protokoll für die Verarbeitung vorbereitet, war ein Teil meiner Aufgabe. Sie zerlegt das NMEA-Protokoll in seine Bestandteile, so dass diese gleich für die Berechnung des Kurswinkels und der Distanz zwischen mehreren Wegpunkten verwendet werden können. Außerdem haben wir einen externen Speicher angeschlossen. Dieser dient dazu die Wegpunkte abzuspeichern, welche der Zeppelin abfliegen soll. Die Ausgabe der Daten erfolgt über ein LCD-Display 

Hendrik

 

Grundsätzliches zu GPS

 

Geographische Koordinaten

 

Kurswinkel- und Distanzberechnung

 

GPS-Protokoll

NMEA

NMEA-0183 (National Marine Electronics Association). NMEA ist ein Standard um einen Datenaustausch zu ermöglichen.

Beispiel Daten:

$GPGGA

hhmmss.ss

1111.111

a

nnnnn.nnn

b

t

uu

v.v

w.w

M

x.x

M

y.y

zzzz*hh<CR><LF>

$GPGGA

081837.00

4809.0682

N

01133.7959

E

1

04

1.90

-00047

M

047

M

 

*43

Der Beispiel Datensatz ist das GGA- Protokoll. Dieses Protokoll haben wir zur Berechnung des Kurswinkels und der Distanz zwischen zwei Wegpunkten verwendet.

Das Protokoll enthält Daten wie zum Beispiel die Zeit und die aktuelle Position des GPS-Empfängers.

Field #

Description

1

UTC of Position

2,3

Latitude, N (Norden) oder S (Süden)

4,5

Longitude, E (Ost) oder W (West)

6

GPS Qualitäts Indikator 0 = No GPS, 1 = GPS, 2 = DGPS

7

Anzahl der Satelliten

8

Horizontal dilution of precision(HDOP)

9,10

Antennenhöhe in Metern

11,12

Geoidal Separation in Meters, M = Meters

13

Alter der GPS-Daten

14

Differential Reference Station ID

hh

Prüfsumme

 

Hardware

 

Schnittstellen des GPS-Empfängers

 

Die Platine ist ein Prototyp für die Verkabelung des Navigationsrechners und dient als Verbindung von GPS-Empfänger, PC, Datenlogger und Arduino.

Schaltplan:

Die Platine hat folgende Anschlüsse:

  • GPS-Empfänger

  • Port 1 (TSIP)

  • Port2 (NMEA)

  • Port 3 (Datenlogger)

  • Stromversorgung (Akku-Pack)

  • Back-Up-Batterie 12 V

     

Externe Stromversorgung

Damit das System auch ohne einen angeschlossenen PC funktioniert haben wir eine externe Stromversorgung angeschlossen. Diese besteht aus einem Akkupack und einem Spannungswandler (Pololu S8V3A Step-Up/Step-Down Voltage Regulator(RB-Pol-113)).

 

Schaltplan:

Der Spannungsregler soll die 3,7 V des Akkuspacks regeln. Für die Stromversorgung des Arduino soll er die 3,7 V auf 5 V regeln. Für die Stromversorgung des GPS-Empfängers regelt der Spannungswandler 3,7 V auf 5V.

Mit einem Schalter auf der Schnittstellenplatine kann man zwischen dem Akkupack und der Stromversorgung durch den PC wechseln.

 

Externer Speicher

 

LCD-Display als Ausgabegerät

Das LCD-Display soll Daten wie den aktuellen Wegpunkt und Kurswinkel ausgeben.

Schaltplan:

 

Taster zur Menüsteuerung

 

Vollständiger Aufbau

Software

 

Parser für NMEA-Protokoll

 

Testparser mit einem Protokollstring


In dieser Version werden die Daten dem Parser über den Serialmonitor übergeben und in einem Datenfeld gespeichert.

Anschließend zerlegt der Parser dann die Daten und speichert sie in den dafür vorgesehenen Variablen ab.

Zum Schluss werden die Character dann noch in Floats umgewandelt

  • Beispiel:
    Testprotokoll: $GPGGA,081837.00,4809.0682,N,01133.7959,E,1,04,1.90,-00047,M,047,M,,*43

 

/*
14.08.2012

Programm um ein $GPGGA Protokoll über die virtuelle Konsole einlesen zu können.
Der Arduino soll die Daten statt vom GPS-Empfänger über die virtuelle konsole erhalten
und diese geordnet ausgeben.

*/


 #include <string.h>
 #include <ctype.h>
 #include <SoftwareSerial.h>
 #include <stdio.h>
  
 
 int i=0, x=0, y;
 int rxPin = 0;                           //Eingang festlegen
 int txPin = 0;                           //Ausgang festlegen
 char Daten[71]="";
 char KennungGPS[7] = "$GPGGA";           //Protokollkennung
 char Test[71]="";
 int Inhalt = 0;
 int richtig = 0;                         //Zähler der zur Überprüfung des Protokolls dient
 int Komma[14];                           //Variable zum Speichern der Positionen der Kommas
 int Sternchen[13];                       //Variable 
 char UTC_h[3],UTC_m[3],UTC_s[6];         //UTC in ASCII
 char LAT_grad[3], LAT_decmin[8];         //Breitengrad in ASCII
 char N_S[2], E_W[2];                     //Himmelsrichtung in ASCII
 char LON_grad[4], LON_decmin[8];         //Längengrad in ASCII
 int n_s,e_w;                             //Himmelsrichtung als Int: N/E -> 1 und S/W -> -1
 
 // umgewandelte Doublewerte
 double utc_h ,utc_m , utc_s , lat_grad , lat_decmin , lon_grad , lon_decmin ;
 
 
 void setup(){
   
   
   pinMode(rxPin, INPUT);             //define pin modes for rx
   pinMode(txPin, OUTPUT);            //define pin modes for tx
 
   for (int i=0;i<72;i++){            //Initialisiere Buffer um Daten aufnehmen zu können
     Daten[i]=' ';
   
   }
 
  Serial.begin(4800);                 //Datenrate des SoftwareSerial port festlegen
 
   int i = 0;                         //Buffer initialisieren der Daten aus der konsole aufnimmt
   while((i<71)&&(Daten[i] != '\n')){ //Daten in den Buffer einlesen
   if(Serial.available()>0){
   Daten[i]= Serial.read();
   Serial.print(Daten[i]); 
   i++;
   }
  }
 
  Serial.println(" ");
  Serial.println(" ");
  
  for(int i = 0;i<7;i++){            //Prüfen ob das richtige Protokoll eingelesen wurde
   if(Daten[i] == KennungGPS[i]){
   richtig++;
   }
  }
  if(richtig==6){
    Serial.println("Richtiges Protokoll");    
    Serial.println(" ");
    for(int i=0;i<71;i++){                   
     if(Daten[i]==','){                               //Prüfen an welcher Stelle die Kommas sind
     Komma[x]=i;
     Serial.print("Stelle des Kommas:"); 
     Serial.println(Komma[x]);
    // Serial.println(Daten[i]);
     x++;
     }
     if(Daten[i]=='*'){                               //Prüfen an welcher Stelle das Sternchen ist 
     Sternchen[x]=i;
     Serial.print("Stelle des Sternchens:");
     Serial.println(Sternchen[x]);
    // Serial.println(Daten[i]);
     x++;
     }  
    }
    // UTC
    strncpy(UTC_h,Daten+7,2);
    strncpy(UTC_m,Daten+9,2);
    strncpy(UTC_s,Daten+11,5);
    UTC_h[2]='\0';
    UTC_m[2]='\0';
    UTC_s[5]='\0';
    Serial.println(UTC_h);
    Serial.println(UTC_m);
    Serial.println(UTC_s);
    
    // Breitengrade
    strncpy(LAT_grad,Daten+17,2);
    strncpy(LAT_decmin,Daten+19,7);
    LAT_grad[2]='\0';
    LAT_decmin[7]='\0';
    Serial.println(LAT_grad);
    Serial.println(LAT_decmin);
    
    //Nord Sued
    strncpy(N_S,Daten+27,1);
    N_S[1]='\0';
    Serial.println(N_S);
    
    if(N_S[0] == 'N'){
      n_s = 1;
     }else{
      n_s = -1; 
     }
    Serial.println(n_s);
    
    //Laengengrade
    strncpy(LON_grad,Daten+29,3);
    strncpy(LON_decmin,Daten+32,7);
    LON_grad[3]='\0';
    LON_decmin[7]='\0';
    Serial.println(LON_grad);
    Serial.println(LON_decmin); 
    
    //Ost West
    strncpy(E_W,Daten+40,1);
    E_W[1]='\0';
    Serial.println(E_W);
    
    if(E_W[0] == 'E'){
      e_w = 1;
     }else{
      e_w = -1; 
     }
     Serial.println(e_w);
     
     //Alle Charakter werden in Floatsumgerechnet
     
     float utc_h = (float)atof(UTC_h);     // char -> double
     Serial.println(utc_h);
     float utc_m = (float)atof(UTC_m);
     Serial.println(utc_m);
     float utc_s = (float)atof(UTC_s);
     Serial.println(utc_s);
     float lat_grad = (float)atof(LAT_grad);
     Serial.println(lat_grad);
     float lat_decmin = (float)atof(LAT_decmin);
     Serial.println(lat_decmin,4);
     float lon_grad = (float)atof(LON_grad);
     Serial.println(lon_grad);
     float lon_decmin = (float)atof(LON_decmin);
     Serial.println(lon_decmin,4);
    }
  }
 
  
 void loop(){                       
 
 }

 

Parser für Datenfluss


Diese Version des Parsers empfängt Daten vom GPS-Empfänger und zerlegt dann die Daten. Diese speichert er wieder in den dafür vorgesehenen Variablen ab. Die Character werden zuletzt noch in Floats umgewandelt.

Um die Daten empfangen zu können wird eine Software-Serielle Schnittstelle initialisiert. Alle Testausgaben erfolgen weiter hin über die Hardwareserielle Schnittstelle.

#include <string.h>
 #include <ctype.h>
 #include "SoftwareSerial.h"
 #include <stdio.h>
 #define RXPin_Soft 2                      //Eine neue serielle Schnittstelle anlegen
 #define TXPin_Soft 3
  
  
 SoftwareSerial mySerial = SoftwareSerial(RXPin_Soft, TXPin_Soft);
 

 int i=0, x=0, y;
 int rxPin = 0;                       //Eingang festlegen
 int txPin = 0;                       //Ausgang festlegen
 char Daten[71]="";                   //Array für NMEA-Protokoll
 char KennungGPS[7] = "$GPGGA";       //Protokollkennung
 char Test[71]="";
 int Inhalt = 0;
 int richtig = 0;                     //Zähler der zur Überprüfung des Protokolls dient
 int Komma[14];                       //Variable zum Speichern der Positionen der Kommas
 int Sternchen[13];                   //Variable 
 char UTC_h[3],UTC_m[3],UTC_s[6];         //UTC in ASCII
 char LAT_grad[3], LAT_decmin[8];         //Breitengrad in ASCII
 char N_S[2], E_W[2];                     //Himmelsrichtung in ASCII
 char LON_grad[4], LON_decmin[8];         //Längengrad in ASCII
 int n_s,e_w;                             //Himmelsrichtung als Int: N/E -> 1 und S/W -> -1

//umgewandelte Integerwerte
 double utc_h ,utc_m , utc_s , lat_grad , lat_decmin , lon_grad , lon_decmin ;
 
 
  void setup(){
   

    pinMode(rxPin, INPUT);             //define pin modes for rx
    pinMode(txPin, OUTPUT);            //define pin modes for tx
 
    pinMode(RXPin_Soft,INPUT);         //Pin's für die Softwareserielle Schnittstelle
    pinMode(TXPin_Soft,OUTPUT);
 
 
    for (int i=0;i<72;i++){            //Initialisiere Buffer um Daten aufnehmen zu können
      Daten[i]=' ';                    //Daten die über die serielle Schnittstelle ausgegeben werden
    
    }
 
   Serial.begin(4800);                 //Datenrate des SoftwareSerial port festlegen
 mySerial.begin(4800);              
 
   int i = 0;                         //Buffer initialisieren der Daten aus der konsole aufnimmt
   while((i<71)&&(Daten[i] != '\n')){ //Daten in den Buffer einlesen
     if(mySerial.available()>0){
       Daten[i]= mySerial.read();
       Serial.print(Daten[i]); 
       i++;
     }
   }


  }
 

  void loop(){ 

    
    for(int i = 0;i<7;i++){            //Prüfen ob das richtige Protokoll eingelesen wurde
      if(Daten[i] == KennungGPS[i]){
       richtig++;                      
     }
    }
    
    if(richtig==6){
      Serial.println("Richtiges Protokoll");    
      Serial.println(" ");
      // UTC
      strncpy(UTC_h,Daten+7,2);
      strncpy(UTC_m,Daten+9,2);
      strncpy(UTC_s,Daten+11,5);
      UTC_h[2]='\0';
      UTC_m[2]='\0';
      UTC_s[5]='\0';
      Serial.println(UTC_h);
      Serial.println(UTC_m);
      Serial.println(UTC_s);
    
      // Breitengrade
      strncpy(LAT_grad,Daten+17,2);
      strncpy(LAT_decmin,Daten+19,7);
      LAT_grad[2]='\0';
      LAT_decmin[7]='\0';
      Serial.println(LAT_grad);
      Serial.println(LAT_decmin);
    
      //Nord Sued
      strncpy(N_S,Daten+27,1);
      N_S[1]='\0';
      Serial.println(N_S);
    
      if(N_S[0] == 'N'){
        n_s = 1;
      }else{
        n_s = -1; 
      }
      Serial.println(n_s);
      
      //Laengengrade
      strncpy(LON_grad,Daten+29,3);
      strncpy(LON_decmin,Daten+32,7);
      LON_grad[3]='\0';
      LON_decmin[7]='\0';
      Serial.println(LON_grad);
      Serial.println(LON_decmin); 
      
      //Ost West
      strncpy(E_W,Daten+40,1);
      E_W[1]='\0';
      Serial.println(E_W);
    
      if(E_W[0] == 'E'){
        e_w = 1;
      }else{
        e_w = -1; 
      }
      Serial.println(e_w);
   
     //Alle Charakter werden in Floatsumgerechnet   
   
     float utc_h = (float)atof(UTC_h);     // char -> double
     Serial.println(utc_h);
     float utc_m = (float)atof(UTC_m);
     Serial.println(utc_m);
     float utc_s = (float)atof(UTC_s);
     Serial.println(utc_s);
     float lat_grad = (float)atof(LAT_grad);
     Serial.println(lat_grad);
     float lat_decmin = (float)atof(LAT_decmin);
     Serial.println(lat_decmin,4);
     float lon_grad = (float)atof(LON_grad);
     Serial.println(lon_grad);
     float lon_decmin = (float)atof(LON_decmin);
     Serial.println(lon_decmin,4);
   }
 
  }

 

Rechnung mit sphärischen Koordinaten

 

Abstand und Kurswinkel zweier Punkte

 

ohne Eingabe über serielle Schnittstelle


Gegeben sind hier zwei Punkte A und B in geographischen Koordinaten und zwar umgerechnet in Grad mit Dezimalstellen. Das Programm berechnet die Distanz der zwei Punkte und den Kurswinkel. Hierbei wird Punkt A als Startpunkt gewählt.

#include "math.h"

double pos_breite_A= 48.149073*PI/180, pos_laenge_A=11.567052*PI/180;  //Koordinaten Punkt A mit Umwandlung in Bogenmaß
double pos_breite_B= 48.149034*PI/180, pos_laenge_B= 11.567433*PI/180; //Koordinaten Punkt B mit Umwandlung in Bogenmaß


void setup() {                
    Serial.begin(9600);
    double zentriw, distance, kurswinkel;
      

      zentriw = 2.0*asin(sqrt(square(sin((pos_breite_A-pos_breite_B)/2.0)) + cos(pos_breite_A)*cos(pos_breite_B)*square(sin((pos_laenge_B-pos_laenge_A)/2.0))));
      distance = zentriw*6370; // in km
      
      Serial.print("Abstand in km: ");
      Serial.println(distance,5);
    
      kurswinkel = acos((sin(pos_breite_B)-sin(pos_breite_A)*cos(zentriw))/(cos(pos_breite_A)*sin(zentriw)));
      Serial.print("Kurswinkel in Grad: ");
      
      // Fallunterscheidung für die Lage der Punkte A und B zueinander
      if(pos_laenge_A <= pos_laenge_B && (pos_laenge_B-pos_laenge_A <= PI)){
        Serial.println(kurswinkel*180/PI);
      }else if(pos_laenge_A > pos_laenge_B && (pos_laenge_A-pos_laenge_B <= PI)){
        Serial.println(360 - (kurswinkel*180/PI));
      }else if(pos_laenge_A < pos_laenge_B && (pos_laenge_B-pos_laenge_A > PI)){
        Serial.println(360 - kurswinkel*180/PI);
      }else if(pos_laenge_A > pos_laenge_B && (pos_laenge_A-pos_laenge_B > PI)){
        Serial.println(kurswinkel*180/PI);
      }
}


void loop() {

}

 

mit Eingabe über serielle Schnittstelle

In dieser Funktion kann man selber Wegpunkte über den seriellen Monitor eingeben und die Entfrenung und den Kurswinkel berechnen lassen. Die Eingabe ist als Zustandsmaschine umgesetzt.

Eingabe IMMER als Gleitkommazahl eingeben! Nur die Minuten haben Dezimalstellen.

Beispiel:
Breitengrad von Position A: 47° 8,45345'

Breite A (Grad)?
> 47.0
Breite A (Minuten)?
> 8.45345

#include "math.h"
  char buffer[11];
  int Zustand;
  double pos_breite_Ag, pos_breite_Am, pos_laenge_Ag, pos_laenge_Am, pos_breite_Bg, pos_breite_Bm, pos_laenge_Bg, pos_laenge_Bm;
  double pos_breite_Ab, pos_laenge_Ab, pos_breite_Bb, pos_laenge_Bb;
  double zentriw, distance, kurswinkel;

double readbuffer();
void setup(){
  Serial.begin(9600);
  Zustand=0;
}

void loop(){
  
  switch(Zustand){
    case 0:{
      //Serial.println(Zustand);
      Serial.println("Breite A (Grad)? ");
      pos_breite_Ag = readbuffer();
      //Serial.println(pos_breite_Ag,4);
      Zustand = 1;
    }break;
    case 1:{
      //Serial.println(Zustand);
      Serial.println("Breite A (Minuten)? ");
      pos_breite_Am = readbuffer();
      pos_breite_Ag = pos_breite_Ag + pos_breite_Am/60;
      //Serial.println(pos_breite_Ag,4);
      pos_breite_Ab = pos_breite_Ag*PI/180;
      Zustand = 2;
    }break; 
    case 2:{
      //Serial.println(Zustand);
      Serial.println("Länge A (Grad)? ");
      pos_laenge_Ag = readbuffer();
      //Serial.println(pos_laenge_Ag,4);
      Zustand = 3;
    }break;
    case 3:{
      //Serial.println(Zustand);
      Serial.println("Länge A (Minuten)? ");
      pos_laenge_Am = readbuffer();
      pos_laenge_Ag = pos_laenge_Ag + pos_laenge_Am/60;
      //Serial.println(pos_laenge_Ag,4);
      pos_laenge_Ab = pos_laenge_Ag*PI/180;
      Zustand = 4;
    }break; 
    case 4:{
      //Serial.println(Zustand);
      Serial.println("Breite B (Grad)? ");
      pos_breite_Bg = readbuffer();
      //Serial.println(pos_breite_Bg,4);
      Zustand = 5;
    }break;
    case 5:{
      //Serial.println(Zustand);
      Serial.println("Breite B (Minuten)? ");
      pos_breite_Bm = readbuffer();
      pos_breite_Bg = pos_breite_Bg + pos_breite_Bm/60;
      //Serial.println(pos_breite_Bg,4);
      pos_breite_Bb = pos_breite_Bg*PI/180;
      Zustand = 6;
    }break; 
    case 6:{
      //Serial.println(Zustand);
      Serial.println("Länge B (Grad)? ");
      pos_laenge_Bg = readbuffer();
      //Serial.println(pos_laenge_Bg,4);
      Zustand = 7;
    }break;
    case 7:{
      //Serial.println(Zustand);
      Serial.println("Länge B (Minuten)? ");
      pos_laenge_Bm = readbuffer();
      pos_laenge_Bg = pos_laenge_Bg + pos_laenge_Bm/60;
      //Serial.println(pos_laenge_Bg,4);
      pos_laenge_Bb = pos_laenge_Bg*PI/180;
      Zustand = 8;
    }break;     
    case 8:{
      //Serial.println(Zustand);
      
      
      
      zentriw = 2.0*asin(sqrt(square(sin((pos_breite_Ab-pos_breite_Bb)/2.0)) + cos(pos_breite_Ab)*cos(pos_breite_Bb)*square(sin((pos_laenge_Bb-pos_laenge_Ab)/2.0))));
      distance = zentriw*6370; // in km
    
      Serial.print("Abstand in km: ");
      Serial.println(distance);
    
      kurswinkel = acos((sin(pos_breite_Bb)-sin(pos_breite_Ab)*cos(zentriw))/(cos(pos_breite_Bb)*sin(zentriw)));
    
      Serial.print("Kurswinkel in Grad: ");
      if(pos_laenge_Ab <= pos_laenge_Bb && (pos_laenge_Bb-pos_laenge_Ab <= PI)){
        Serial.println(kurswinkel*180/PI);
      }else if(pos_laenge_Ab > pos_laenge_Bb && (pos_laenge_Ab-pos_laenge_Bb <= PI)){
        Serial.println(360 - (kurswinkel*180/PI));
      }else if(pos_laenge_Ab < pos_laenge_Bb && (pos_laenge_Bb-pos_laenge_Ab > PI)){
        Serial.println(360 - kurswinkel*180/PI);
      }else if(pos_laenge_Ab > pos_laenge_Bb && (pos_laenge_Ab-pos_laenge_Bb > PI)){
        Serial.println(kurswinkel*180/PI);
      }     
          
      Zustand = 0;
    }
  }
}

//liest die floatWerte ein 
double readbuffer(){
  double erg;
  int ergvor, ergdezi;
  int i =0;
  while((i<10) && (buffer[i-1] != '\n')){
    if(Serial.available()){
      buffer[i]=Serial.read();
      //Serial.println(buffer[i]);
      i++; 
    }
  }
  sscanf((char *)buffer, "%d.%d", &ergvor, &ergdezi);
  //Serial.println(ergdezi);
  erg = ergdezi;

  erg=erg/10000;

  erg= erg+ergvor;
  
  
  return erg;
}

 

Speicherverwaltung im EEPROM


Um nicht jedes Mal ein Array neu zu bescheiben und dann den Arduion zu flashen, bedienen wir uns eines EEPROM, der extern beschrieben werden kann. Er wird schließlich nur mit den Wegpunkten an den Arduino angesteckt und dort dann ausgelesen. Später soll es möglich sein neue Wegpunkte mit der Bodenstation, während des Fluges zu editieren. Daher sind sind die Funktionen entsprechend allgemein gehalten und wiederverwendbar.

Struktur


Zu nächst haben wir die Struktur unseres Speichers festgelegt. Insgesamt haben wir 4096 Byte Speicher. Diesen habe wir so eingeteilt:

V Lat Lon Alt Status Stuff N

 

  • V: Adresse des vorherigen Wegpunktes (1 Byte)
  • Lat: Breitengrade (4 Byte)
  • Lon: Längengrade (4 Byte)
  • Alt: Höhe (4 Byte)
  • Status: Orientierung der Koordinaten und weitere Prüfbits (1 Byte)
  • Stuff...... (1 Byte)
  • N: Adresse des Nächsten Wegpunktes (1 Byte)


Das Statusbyte ist noch wie folgt in 8 Bits unterteilt:

N/S E/W X X X Start Valid End

 

  • N/S: Nördliche(1) oder südliche(0) Breitengrade (akt. Position)
  • E/W: Östlicher(1) oder westlicher(0) Längengrade (akt. Position)
  • X: Unbenutztes Bit
  • Start: Gibt an, ob es sich um den ersten Wegpunkt in der Kette handelt
  • Valid: Gibt an, ob die Koordinatengültig sind
  • End: Gibt an, ob es sich um den letzten Wegpunkt in der Kette handelt


Wir haben also 16 Byte für jeden Datenblock. Mit dieser Einteilung können wir 255 Wegpunkte speichern.

Der erste Datenblock (16 byte) gibt uns die Adresse auf den ersten Wegpunkt und weitere wichtige Statusinformationen.

X Status X N

 

  • X: Nonsens
  • Status:
    0 0 0 0 0 0 0 End
    (1 byte; Adresse: 13)
    • End: eine 1 an, dass eine Wegpunktliste angehängt ist
  • N: Adresse des Starblocks (1byte; Adresse: 15)

Funktionen

 

Ein Byte in den EEPROM einlesen oder auslesen

 

//Ein Byte in den EEPROM einlesen
void i2c_eeprom_write_byte( int deviceaddress, unsigned int eeaddress, byte data ) {
  int rdata = data;
  Wire.beginTransmission(deviceaddress);
  Wire.write((int)(eeaddress >> 8));   // MSB
  Wire.write((int)(eeaddress & 0xFF)); // LSB
  Wire.write(rdata);
  Wire.endTransmission();
  delay(10);
}

 

//Ein Byte aus dem EEPROM lesen
byte i2c_eeprom_read_byte( int deviceaddress, unsigned int eeaddress ) {
  byte rdata = 0xFF;
  Wire.beginTransmission(deviceaddress);
  Wire.write((int)(eeaddress >> 8)); // MSB
  Wire.write((int)(eeaddress & 0xFF)); // LSB
  Wire.endTransmission();
  Wire.requestFrom(deviceaddress,1);
  if (Wire.available()) rdata = Wire.read();
  return rdata;
}

 

Read_Waypoint


Übergabewerte: Adresse des Wegpunktes(byte)
Rückgabewert: Wegpunkt(struct waypoint)

Diese Funktion liest einen Wegpunkt aus dem Speicher aus.

struct waypoint read_waypoint( byte working_address){
    
  byte b0, b1, b2, b3;  // Variablen für die Umwandlung von 4 bytes in einen Float
  byte buffer;          // Puffer variable
  int address;          // Variable die der Lesefunktion übergeben wird und die Adresse der Daten im Speicher angibt
  byte Status;          // Variable um das Statusbyte abzuspeichern
  struct waypoint wp;   // Variable des Typs struct Way point um auf die ... Zuzugreifen...
  
  wp.last_wp = i2c_eeprom_read_byte(0x50,address);    //Auslesen des letzten Wegpunktes
 // Serial.println(wp.last_wp);
  
  address = working_address * 16;
   
  b0 = i2c_eeprom_read_byte(0x50,address+1);
  b1 = i2c_eeprom_read_byte(0x50,address+2);
  b2 = i2c_eeprom_read_byte(0x50,address+3);
  b3 = i2c_eeprom_read_byte(0x50,address+4);
  
  wp.latitude = bytesToFloat( b0, b1, b2, b3);               //Auslesen der Breitengrade
 // Serial.println(wp.latitude);
  
  b0 = i2c_eeprom_read_byte(0x50,address+5);
  b1 = i2c_eeprom_read_byte(0x50,address+6);
  b2 = i2c_eeprom_read_byte(0x50,address+7);
  b3 = i2c_eeprom_read_byte(0x50,address+8);

  wp.longitude = bytesToFloat( b0, b1, b2, b3);              //Auslesen der Laengengrade
 // Serial.println(wp.longitude);
  
  b0 = i2c_eeprom_read_byte(0x50,address+9);
  b1 = i2c_eeprom_read_byte(0x50,address+10);
  b2 = i2c_eeprom_read_byte(0x50,address+11);
  b3 = i2c_eeprom_read_byte(0x50,address+12);

  wp.altitude = bytesToFloat( b0, b1, b2, b3);                //Auslesen der Hoehe
 // Serial.println(wp.altitude);
  
  
  Status = i2c_eeprom_read_byte(0x50,address+13);             //Auslesen des Statusbytes
  
  Serial.print("Stat: ");
  Serial.println(Status);
  
  wp.next_wp = i2c_eeprom_read_byte(0x50,address+15); //Auslesen des nächsten Wegpunktes
 // Serial.println(wp.next_wp);
  

  // Das Statusbyte in bits zerlegen und die Bit in den passenden Variablen abspeichern
  
  // N/S und E/W in Variablen speichern
  buffer = Status >> 7;
  wp.geo_direct_lat = buffer;  // N_S aus Speicher
 // Serial.println(wp.geo_direct_lat);
  
  buffer = Status << 1;
  buffer = buffer >> 7;
  wp.geo_direct_lon = buffer;  // E_W aus Speicher
 // Serial.println(wp.geo_direct_lon);
  
   buffer = Status << 5;        // Abfragen ob es sich um den ersten Wegpunkt handelt
  buffer = buffer >> 7;
  wp.start_wp = buffer;
    
  //Valid bit auslesen
  buffer = Status << 6;
  buffer = buffer >> 7;
  wp.valid = buffer;           // Validbit aus Speicher
 // Serial.println(wp.valid);
  
  //buffer = Status & 1;
  buffer = Status << 7;
  //Serial.println(buffer);
  buffer = buffer >> 7;
  //Serial.println(buffer);
  wp.end_wp = buffer;
  //Serial.print("End: ");
  //Serial.println(wp.end_wp);
  
  return wp;
}


float bytesToFloat(byte b0, byte b1, byte b2, byte b3)
{
float output;

*((byte*)(&output) + 0) = b0;
*((byte*)(&output) + 1) = b1;
*((byte*)(&output) + 2) = b2;
*((byte*)(&output) + 3) = b3;

return output;
}

 

Read_2_Waypoint


Übergabewerte: Adresse des Wegpunktes(byte)
Rückgabewerte: Adresse des vorrausgegangenen Wegpunktes und des nächsten Wegpunktes; Markierung ob es sich um den letzten oder ersten Wegpunkt der Kette handelt(struct chain_link).

Diese Funktion liest die Adresse des vorherigen Wegpunktes, des nächsten Wegpunktes und das Statusbyte aus. Außerdem wird geprüft ob es sich um den letzten oder ersten Wegpunkt handelt

struct chain_link read_2(byte working_address){
  
  int address;          // Variable die der Lesefunktion übergeben wird und die Adresse der Daten im Speicher angibt
  byte Status;          // Variable um das Statusbyte abzuspeichern
  byte buffer;          // Puffer variable
  
  struct chain_link cl;
  
  address = working_address * 16;
  
  cl.last_wp = i2c_eeprom_read_byte(0x50,address);    //Auslesen des letzten Wegpunktes
  cl.next_wp = i2c_eeprom_read_byte(0x50,address+15); //Auslesen des nächsten Wegpunktes
  Status = i2c_eeprom_read_byte(0x50,address+13);     //Auslesen des Statusbytes
  buffer = Status << 7;                               //Auslesen ob es sich um den letzten Wegpunkt handelt
  buffer = buffer >> 7;
  cl.end_wp = buffer;
  
  buffer = Status << 5;        // Abfragen ob es sich um den ersten Wegpunkt handelt
  buffer = buffer >> 7;
  cl.start_wp = buffer;
    
  return cl;
}

 

Write_Waypoint


Übergabewerte: Adresse des Wegpunktes(byte), Wegpunkt(struct waypoint)
Rückgabewert:none

//Einen Wegpunkt in den Speicher schreiben
void write_waypoint(struct waypoint wp, byte working_address){
  int address;
  byte lat[4], lon[4], alt[4];
  byte stat = 0, buffer = 0;
  address = working_address * 16;
  
  sepperate_float(wp.latitude, lat);
  sepperate_float(wp.longitude, lon);
  sepperate_float(wp.altitude, alt);
  
  //Statusbyte zusammenbasteln
  stat = wp.end_wp;
  buffer = wp.valid << 1;
  stat = buffer | stat;
  buffer = wp.start_wp << 2;
  stat = buffer | stat;
  buffer = wp.geo_direct_lon << 6;
  stat = buffer | stat;
  buffer = wp.geo_direct_lat << 7;
  stat = buffer | stat;
  
  
  
  //byteweise einlesen
  i2c_eeprom_write_byte( 0x50, address, wp.last_wp);
  i2c_eeprom_write_byte( 0x50, address+1, lat[0]);
  i2c_eeprom_write_byte( 0x50, address+2, lat[1]);
  i2c_eeprom_write_byte( 0x50, address+3, lat[2]);
  i2c_eeprom_write_byte( 0x50, address+4, lat[3]);
  i2c_eeprom_write_byte( 0x50, address+5, lon[0]);
  i2c_eeprom_write_byte( 0x50, address+6, lon[1]);
  i2c_eeprom_write_byte( 0x50, address+7, lon[2]);
  i2c_eeprom_write_byte( 0x50, address+8, lon[3]);
  i2c_eeprom_write_byte( 0x50, address+9, alt[0]);
  i2c_eeprom_write_byte( 0x50, address+10, alt[1]);
  i2c_eeprom_write_byte( 0x50, address+11, alt[2]);
  i2c_eeprom_write_byte( 0x50, address+12, alt[3]);
  i2c_eeprom_write_byte( 0x50, address+13, stat);
  //ein Byte ist hier noch frei
  i2c_eeprom_write_byte( 0x50, address+15, wp.next_wp);
  
}
  

void sepperate_float(float  float_value, byte buf[4]){
  long int_value, buffer;
  int i;
  
  int_value = *((long *) &float_value);
  //Serial.print("int: ");
  //Serial.println(int_value);
  
  for(i=3; i>=0; i--){
    buffer = int_value >> (i*8);
    buf[i] = 0xFF & buffer;
  }
  
}



Diese Funktion schreibt einen Wegpunkt in den Speicher.

Rewrite_Waypoint


Übergabewerte:Adresse des zu bearbeitenden Wegpunktes(byte), neuer Wegpunkt (struct Waypoint)
Rückgabewert: none

Diese Funktion bearbeitet einen Wegpunkt. Falls der Wegpunkt gearade aktuell ist, dann muss der Wegpunkt auch auf dem Arduino geändert werden.

void rewrite_waypoint(int wegpunkt_nr,struct waypoint wp_2){
 int a = 0;           //Laufparameter
 byte waypoint = 0;        //Adresse des Wegpunktes
 byte working_address; //Adresse des gewünschten Wegpunktes
 struct chain_link wp_1;
 
 // Suche den Richtigen Wegpunkt in der Liste, wegpunkt nummer ist die Nummer des wp nicht Adresse!
 for(a = 0; a < wegpunkt_nr; a++){  
     waypoint = next_waypoint(waypoint);           // Funktionsaufruf von next_waypoint
 }
 
  working_address = waypoint;
  Serial.print("Working_address: ");
  Serial.print(working_address);
  
  wp_1 = read_2(working_address);

  wp_2.next_wp = wp_1.next_wp;
  wp_2.last_wp = wp_1.last_wp;
  wp_2.start_wp = wp_1.start_wp;
  wp_2.end_wp = wp_1.end_wp;
  wp_2.valid = wp_1.valid;
  write_waypoint(wp_2, working_address);
}

 

Add_Waypoint

Übergabewerte:Wegpunkt(struct waypoint)
Rückgabewert:none

Diese Funktion hängt einen Wegpunkt hinten an der Kette an. Ist kein freier Speicher vorhanden, wird eine Fehlermeldung über den Serial Monitor ausgegeben.

void add_waypoint(struct waypoint working_waypoint)
{
  struct waypoint wp;
  struct chain_link cl;
  
  byte new_wp_address = find_free_memory();          //Adresse auf freien Speicherblock
  byte address, old_address;
  
  Serial.println(new_wp_address);
  
  if(new_wp_address != 0){                           //Abbruch bei fehlendem Speicher
    
    cl = read_2(0);                                    //Endbit aus 0. Block auslesen
    if(cl.end_wp == 0){                                //Wegpunkt ist nicht der Erste in der Liste
      address = next_waypoint(0);
      old_address = 0;
      
      while(address != old_address){                   //Letzten Wegpunkt in der Liste finden
        old_address = address;
        address = next_waypoint(address);
      }
      
      working_waypoint.last_wp = address;              //Zeiger von neuem Wegpunkt auf letzten Wegpunkt
      write_waypoint_nextadd(new_wp_address, address); //Zeiger von letztem Wegpunkt auf neuen Wegpunkt
      
      wp = read_waypoint(address);                     //Endbit im ehemaligen letzten Wegpunkt auf 0 setzen
      wp.end_wp=0;
      write_waypoint(wp, address);
      
      working_waypoint.start_wp = 0;                   //Startbit im neuen Wegpunkt setzen
      
    }else{                                             //Wegpunkt ist der Erste in der Liste
      
      working_waypoint.last_wp = new_wp_address;       //Zeiger auf sich selbst
      write_waypoint_nextadd(new_wp_address, 0);      //Zeiger vom 0. Block auf den Startwegpunkt
      
      i2c_eeprom_write_byte(0x50, 13, 0);              //Endbit im 0. Block auf 0 setzen => Elemente in der Liste vorhanden
      delay(10);
      
      working_waypoint.start_wp = 1;                   //Startbit im neuen Wegpunkt setzen. 1 da es sich um den ersten Wegpunkt handelt
    }
     working_waypoint.valid = 1;                       //Validbit im neuen Wegpunkt setzen
     working_waypoint.end_wp = 1;                      //Endbit im neuen Wegpunkt setzen
     
    write_waypoint(working_waypoint,new_wp_address);
  }else Serial.println("Kein Speicher frei");
}

 

Insert_Waypoint


Übergabewerte:Adresse des Wegpunktes(byte),Wegpunkt(struct waypoint)
Rückgabewert:none

Diese Funktion fügt einen Wegpunkt in unsere verkettete Liste ein.
Später sollen die Wegpunkte während des Fluges über die Bodenstation des Zeppelins eingefügt werden sollen.

void insert_waypoint(int waypoint_nr_last, int waypoint_nr_next, struct waypoint working_waypoint)
{
  int i;
  struct waypoint wp;
  byte waypoint_last_add = 0, waypoint_next_add = 0; //Adressen des Vorgängers und Nachfolgers
  byte new_wp_address = find_free_memory();          //Adresse auf freien Speicherblock
  
  if(waypoint_nr_last+1 == waypoint_nr_next){
    
    if(new_wp_address != 0){                           //Abbruch bei fehlendem Speicher
      
      if(waypoint_nr_last!=0){                                     //der neue Wegpunkt ist nicht der zukünftige erste Wegpunkt
      
        for(i=0; i < waypoint_nr_last; i++){                     //Suche der Adresse zum vorherigen Wegpunktes
          waypoint_last_add = next_waypoint(waypoint_last_add);
        }
        
        waypoint_next_add = next_waypoint(waypoint_last_add);        //der nachfolgende Wegpunkt ist momentan noch direkt hinter dem Vorgänger
     
        write_waypoint_nextadd(new_wp_address, waypoint_last_add); //Zeiger von Vorgänger auf neuen Wegpunkt
        write_waypoint_lastadd(new_wp_address, waypoint_next_add); //Zeiger von Nachfolger auf neuen Wegpunkt
        working_waypoint.last_wp = waypoint_last_add;              //Zeiger von neuem Wegpunkt auf den Vorgänger
        working_waypoint.next_wp = waypoint_next_add;              //Zeiger von neuem Wegpunkt auf den Nachfolger
        
        working_waypoint.valid = 1;                                //Neuer Wegpunkt ist jetzt gültig
        working_waypoint.end_wp = 0;                               //Neuer Wegpunkt wird nie am Ende eingefügt
        working_waypoint.start_wp = 0;                             //Neuer Wegpunkt ist in diesem Fall nicht der erste Wegpunkt
        
      }else{                                                       //der neue Wegpunkt ist der zukünftige erste Wegpunkt
        
        waypoint_next_add = next_waypoint(0);                      //der nachfolgende Wegpunkt der erste Wegpunkt
        
        write_waypoint_nextadd(new_wp_address,0);                  //Zeiger von außen auf neuen Wegpunkt biegen
        write_waypoint_lastadd(new_wp_address,waypoint_next_add);
        working_waypoint.last_wp = new_wp_address;                 //Zeiger von neuem Wegpunkt auf sich selbst
        working_waypoint.next_wp = waypoint_next_add;              //Zeiger von neuem Wegpunkt auf den Nachfolger

        working_waypoint.valid = 1;                                //Neuer Wegpunkt ist jetzt gültig
        working_waypoint.end_wp = 0;                               //Neuer Wegpunkt wird nie am Ende eingefügt
        working_waypoint.start_wp = 1;                             //Neuer Wegpunkt ist in diesem Fall der erste Wegpunkt        
        
        wp = read_waypoint(waypoint_next_add);                     //Startbit im ehemaligen 1. Wegpunkt auf 0 setzen
        wp.start_wp=0;
        write_waypoint(wp,waypoint_next_add);
        
        if(waypoint_next_add == act_address){                      //Abfrage, ob der Nachfolger gerade angesteuert wird, wenn ja wird jetzt  der Neue angeflogen
          act_address = new_wp_address;
          act_waypoint = working_waypoint;
        }
      }
      
      write_waypoint(working_waypoint,new_wp_address);             
      
    }else Serial.println("Kein Speicher frei");
  }else Serial.println("Falsche Eingabe");
}

 

Delete_Waypoint


Übergabewerte:Adresse des Wegpunktes (byte), der gelöscht wird
Rückgabewert:none

Die Funktion Delete_Waypoint löscht einen bestimmten Wegpunkt in der verketteten Liste

Next_Waypoint


Übergabewerte: Adresse des aktuellen Wegpunktes (byte)
Rückgabewert: Adresse des nächsten Wegpunktes (byte)

Diese Funktion gibt die Adresse des nächsten Wegpunktes zurück und überprüft, ob es sich beim nächsten Wegpunkt um den Letzten handelt.

//Gibt augehend von der übergebenen Adresse, die folgende Adresse aus.	 
byte next_waypoint(byte working_address)	 
{	 
   struct chain_link cl;	 
	 
   cl = read_2(working_address);	 
	 
   if(cl.end_wp == 0){	 
      return cl.next_wp;	 
   }else return working_address; //Gibt die Adresse des momentanen Blocks zurück, falls das Ende der Kette erreicht wurde.	 
}

 

Last_Waypoint


Übergabewerte: Adresse des aktuellen Wegpunktes (byte)
Rückgabewert: Adresse des vorherigen Wegpunktes (byte)

Diese Funktion gibt die Adresse des vorherigen Wegpunktes zurück, es sei denn es ist der Erste.

//Gibt augehend von der übergebenen Adresse, die vorhergehende Adresse aus.	 
byte last_waypoint(byte working_address)	 
{	 
   struct chain_link cl;	 
	 
   cl = read_2(working_address);	 
	 
   if(cl.start_wp == 0){	 
      return cl.last_wp;	 
   }else return working_address; //Gibt die Adresse des momentanen Blocks zurück, falls der Anfang der Kette erreicht wurde. 	 
}

 

Hilfsfunktionen

 

//Freien Speicher finden, ansonsten wird 0 ausgegeben
byte find_free_memory()
{
  struct chain_link cl;
  int i;
  byte rueck;
  
  for(i = 1; i<256; i++){
    cl = read_2((byte) i);
    if(cl.valid == 0){
      break;
    }
  }
  
  if(i==256){
    return 0;
  }  
  
  rueck =  (byte) i;
  return rueck; 
} 


//Löscht die Wegpunktliste, aber nicht den Speicher
void delete_waypointlist()
{
  i2c_eeprom_write_byte(0x50, 13, 1);
  delay(10);
}

//Gibt den Spreicher wieder frei. Statusbytes werden auf Null gesetzt
void free_memory()
{
  int i;
  
  for(i=1;i<256;i++){
    i2c_eeprom_write_byte(0x50, (i*16)+13, 0);
    delay(10);
  }
  
  delete_waypointlist();
}

//Schreibt in einem bestimmten Block die Nachfolgeradresse
void write_waypoint_nextadd(byte nextadd, byte working_address)
{
  int address = working_address*16;
  i2c_eeprom_write_byte( 0x50, address+15, nextadd);  
}

//Schreibt in einem bestimmten Block die Vorgängeradresse
void write_waypoint_lastadd(byte lastadd, byte working_address){
  int address = working_address*16;
  i2c_eeprom_write_byte( 0x50, address, lastadd);
}
//Funktion gibt die Nummer eines Wegpunktes an einer bestimmten Adresse zurück wenn ein Fehler auftritt gibt die Funktion 0 zurück
int number_of_waypoint(byte working_address){
  int nr = 0;
  byte a = 0, b = 0, c = 0;         //Laufparameter
  byte waypoint;             //Adresse des Wegpunktes
  struct chain_link wp_1;    //Variablen des Types struct chain_link
  
  
  for(a = next_waypoint(0); b != working_address; a= next_waypoint(a)){  // Suche den Richtigen Wegpunkt in der Liste 
    if(a!=c){
    wp_1 = read_2(a);
    b = wp_1.next_wp;
    next_waypoint(working_address);
    nr++;
    c = a;
    }else{
    return 0;
    }
  }
  
 return nr;
}

 

Ansteuerung der Taster

Das folgende Programm basiert auf einer Schaltung mit zwei Tastern. Sie dienen dazu einen Zähler zu erhöhen und zu verringern. Damit dies pro Tastendruck nur einmal passiert, wird die Taste entprellt.
Bei längerem Drücken der Taster wird der Zähler auf einen festen Wert gesetzt (hier: 0 oder 100). Das Programm ist ein ein Test zum Umgang mit den Tastern gewesen.
In der Final-Version ist schließlich der endgültige Einsatz zu sehen. (WICHTIG: Funktioniert in der Finalversion noch nicht!) Dort sollten die Wegpunkte ausgewählt werden können und bei längerem Drücken entweder der nächste Wegpunkt oder die aktuelle Position angezeigt werden.

#define ENTPRELLEN 50
#define GEDRUECKT 1000

const int buttonPin1 = 8, buttonPin2 = 9;     
int buttonState1, buttonState2 = 0;
int drueck_zeit1 = 0, drueck_zeit2 = 0, start1 = 0, start2 = 0;
int taste1 = 0, taste2 = 0;
int done1 = 2, done2 = 2;

int zaehler=0;

void setup() {  
  Serial.begin(4800);  
  pinMode(buttonPin1, INPUT);
  pinMode(buttonPin2, INPUT); 
  Serial.println(zaehler); 
}

void loop(){
  
  //Timer für Button1 - Start
  if((digitalRead(buttonPin1) == HIGH) && (taste1 == LOW) && (done1 == 2)){
    start1=millis();
    taste1 = HIGH;
    done1 = 0;
  }
  
  //Timer für Button1 - gedrueckt
  if((digitalRead(buttonPin1) == HIGH) && (taste1 == HIGH))
  {
    drueck_zeit1 = millis()-start1;
  }
  
  
  if((digitalRead(buttonPin1) == LOW) && (taste1 == HIGH) && (drueck_zeit1 > ENTPRELLEN) && (drueck_zeit1 <= GEDRUECKT)){    
    if((zaehler-1)>=0){
      zaehler--;
    }
    Serial.println(zaehler);
    done1 = 1;
  }
  
  if((taste1 == HIGH) && (drueck_zeit1 > GEDRUECKT) && (done1 == 0)){
    zaehler = 0;
    Serial.println(zaehler);
    done1 = 1;
  }
  
  if((digitalRead(buttonPin1) == LOW) && (taste1 == HIGH) && (done1 == 1)){
    drueck_zeit1 = 0;
    done1 = 2;
    taste1 = LOW;
  }
  
  
  
  //Timer für Button2 - Start
  if((digitalRead(buttonPin2) == HIGH) && (taste2 == LOW) && (done2 == 2)){
    start2=millis();
    taste2 = HIGH;
    done2 = 0;
  }
  
  //Timer für Button2 - gedrueckt
  if((digitalRead(buttonPin2) == HIGH) && (taste2 == HIGH))
  {
    drueck_zeit2 = millis()-start2;
  }
  
  //Button2 kurz gerückt
  if((digitalRead(buttonPin2) == LOW) && (taste2 == HIGH) && (drueck_zeit2 > ENTPRELLEN) && (drueck_zeit2 <= GEDRUECKT)){    
      zaehler++;
    
    Serial.println(zaehler);
    done2 = 1;
  }
  
  //Button2 lang gedrückt
  if((taste2 == HIGH) && (drueck_zeit2 > GEDRUECKT) && (done2 == 0)){
    zaehler = 100;
    Serial.println(zaehler);
    done2 = 1;
  }
  
  
  if((digitalRead(buttonPin2) == LOW) && (taste2 == HIGH) && (done2 == 1)){
    drueck_zeit2 = 0;
    done2 = 2;
    taste2 = LOW;
  }
}

 

Final-Version

 

/*
28.08.2012

Programm um $GPGGA Protokolle die vom GPS-Empfänger gesendet werden  an den Arduino zu übermittelt und diese dort zu verarbeiten. 
Der Arduino soll fehlerhafte Protokolle nicht auswerten.
Aus der Menge an richtigen Protokollen soll der Kurswinkel und der Abstand zum nächsten Wegpunkt berechnet werden. 
Diese Werte Sollen über ein LCD-Display ausgeben werden.
Mit zwei Tastern können die Wegpunkte ausgewählt, die aktuelle Position und
der nächste Wegpunkt angezeigt werden.

*/


 #include <string.h>
 #include <SoftwareSerial.h>
 #include <LiquidCrystal.h>
 #include "math.h"

 #define RXPin_Soft 2                      //Eine neue serielle Schnittstelle anlegen
 #define TXPin_Soft 3
 #define PI 3.14159265
 #define ENTPRELLEN 50
 #define GEDRUECKT 1000
  
 SoftwareSerial mySerial = SoftwareSerial(RXPin_Soft, TXPin_Soft);
 LiquidCrystal lcd(12,11,7,6,5,4);    //Library mit den Nummern der Pins initialisieren
 
 
 const int buttonPin1 = 8, buttonPin2 = 9;                         //Pins für Button 1 und 2
 int buttonState1, buttonState2 = 0;                               //Status von Button 1 und 2
 int drueck_zeit1 = 0, drueck_zeit2 = 0, start1 = 0, start2 = 0;   //Startzeit und Zeit von Button 1 und 2
 int taste1 = 0, taste2 = 0;                                       //Zustandsvariablen von Button 1 und 2
 int done1 = 2, done2 = 2;
 
 
 
 char Daten[71]="";                       //Array für NMEA-Protokoll
 const char KennungGPS[7] = "$GPGGA";     //Protokollkennung
 int richtig = 0;                         //Zähler der zur Überprüfung des Protokolls dient
 char UTC_h[3],UTC_m[3],UTC_s[6];         //UTC in ASCII
 char LAT_grad[3], LAT_decmin[8];         //Breitengrad in ASCII
 char N_S[2], E_W[2];                     //Himmelsrichtung in ASCII
 char LON_grad[4], LON_decmin[8];         //Längengrad in ASCII
 char SATANZ[3], HDOP[5];                 //Satelitenanzahl (diabolische Variable) und Horizontal Dilution of Precision (wird errechnet aus der Position der Sateliten zueinander)
 int  zaehler;                            //Zähler, der die Wegpunkte im Array durchläuft
 int n_s, e_w;                            //Himmelsrichtung als Int: N/E -> 1 und S/W -> -1
 int utc_h, utc_m, satanz;                //umgewandelte Integerwerte
 double utc_s, hdop;                      // umgewandelte Doublewerte
 
 double lat,lat_grad,lat_decmin,lon,lon_grad,lon_decmin; //umgewandelte Integerwerte (lat und lon in Bogenmaß!)
 
 
 double wegpunkte[6][2]={{48.1489586,11.5676057},
                         {48.1479779,11.5669084},
                         {48.1483645,11.5655780},
                         {48.1504260,11.5669620},
                         {48.1502149,11.5681368},
                         {48.1489622,11.5676057}}; //6 Wegpunkte
 
        
 
 double zentriw, distance = 3, kurswinkel; //Zentriwinkel, Abstand zum nächsten Wegpunkt (muss so initialisiert sein, dass while Schleife durchlaufen wird), Kurswinkel (läuft von Norden im Uhrzeigersinn z.B 90° => Osten)
 
 
 //Vordklaration
 void switch_wegpunkte();
 int parser_nmea();
 void berechnung_kurswinkel_distanz();
 
  void setup(){
   
 
    pinMode(RXPin_Soft,INPUT);         //Pins für die Softwareserielle Schnittstelle definieren
    pinMode(TXPin_Soft,OUTPUT);
    
    pinMode(buttonPin1, INPUT);        //Pins für die Buttons 1 und 2 definieren
    pinMode(buttonPin2, INPUT);
 

    for (int i=0;i<71;i++){            //Initialisiere Daten-Array mit Leerzeichen
      Daten[i]=' ';
    }
 
   Serial.begin(4800);                 //Datenrate des HardwareSerial port festlegen
   mySerial.begin(4800);               //Datenrate des SoftwareSerial port festlegen 
   lcd.begin(16,2);                    //LCD-Display mit 16 Spalten und 2 Zeilen
   lcd.print("Setup ");
  
   for(int i=0; i<6;i++){
     for(int j=0; j<2;j++){        
       wegpunkte[i][j]=wegpunkte[i][j] * PI/180; //Umwandlung der Wegpunkte in Bogenmaß
     }
   }
   
   zaehler=0;                          //Wegpunktzähler initialisieren
   lcd.print("fertig");
  }
 

  void loop(){ 
    
    
    while(distance>2){  //Wegpunkt erreicht, wenn der Abstand unter 2 Meter
      
      switch_wegpunkte();
    
      if(zaehler>=6){                    //Abbruch, wenn alle Wegpunkte im Array erreicht wurden
       break;
      }
    
      int i;
     
      for (i=0;i<71;i++){                //Initialisiere Buffer um Daten aufnehmen zu können
      Daten[i]=' ';
      }
      
      i = 0;                             //Buffer initialisieren der Daten aus der konsole aufnimmt
      
      do{                                //Daten in den Buffer einlesen 
        if(mySerial.available() > 0){
          Daten[i]= mySerial.read();
          Serial.print(Daten[i]);
          i++;
        }
      }while((i < 71)&&(Daten[i-1] != '\n'));
      
      richtig = 0;
      
      for(int a = 3;a < 6;a++){         //Prüfen ob das richtige Protokoll eingelesen wurde (GGA im String entahlten?)
        if(Daten[a] == KennungGPS[a]){
         richtig++;                     //ist GGA an der richtigen Stelle, dann ist richtig == 3
       }
      }

    
        
        if(richtig == 3){
          
        if((Daten[6]==',')&&(Daten[16]==',')&&(Daten[26]==',')&&(Daten[28]==',')&&(Daten[39]==',')&&(Daten[41]==',')&&(Daten[43]==',')&&
            (Daten[46]==',')&&(Daten[51]==',')&&(Daten[57]==',')&&(Daten[59]==',')&&(Daten[63]==',')&&(Daten[65]==',')&&(Daten[66]==','))      //zusätzlich Überprüfung duch die richtigen Kommapositionen
        {
            
                               
           if(parser_nmea()){                              //Abbruch bei fehlender Genauigkeit
              
              //RECHNUNG//
              berechnung_kurswinkel_distanz();
              
              //Serial.println(" ");                                                          //Serial-Monitor-Ausgabe wird übersichtlicher
              //Serial.println(" ");
              
              delay(1000);                                                                    //LSD-Display aufräumen 
              lcd.setCursor(0,0);
              lcd.print("                ");
              lcd.setCursor(0,1);
              lcd.print("                ");
              
              lcd.setCursor(0,0);                                                             //Wegpunktzähler auf LSD-Display ausgeben
              lcd.print("WP:");
              lcd.setCursor(4,0);
              lcd.print(zaehler+1);
              lcd.setCursor(6,0);                                                             //360°-Kuswinkel ausgeben
              lcd.print("KW*");         
              lcd.setCursor(10,0);
              lcd.print(360-(kurswinkel*180/PI));
              
              
              lcd.setCursor(0,1);                                                             //Abstand zum nächsten Wegpunkt ausgeben
              lcd.print("d:");
              lcd.setCursor(3,1);
              lcd.print(distance,2);
              
              lcd.setCursor(13,1);                                                            //Satelitenanzahl ausgeben
              lcd.print("S:");
              lcd.print(satanz);
            }else{
              lcd.setCursor(0,0);                                                             //Fehlermeldung für Ungenauigkeit auf dem LSD-Display
              lcd.print("                ");
              lcd.setCursor(0,1);
              lcd.print("                ");
              lcd.setCursor(0,0);
              lcd.print("hdop < 3");
            }
            
         }
     
      }
     
    }
    zaehler++;    
    lcd.setCursor(0,0);                                                                       //Meldung zur erfolgreichen Auffindung eines Wegpunktes auf dem LSD-Display
    lcd.print("                ");
    lcd.setCursor(0,1);
    lcd.print("                ");
    lcd.setCursor(0,0);
    lcd.print("Hurra!");
    lcd.setCursor(0,1);
    lcd.print("Wir sind da!");
    delay(2000);
    
    distance=3; //Damit while-Schleife wieder durchlaufen wird.
    
  }
  
  
  int parser_nmea()
  {
            strncpy(HDOP,Daten+47,4);                                //Beginn des Parsers
            hdop=atof(HDOP);
            
            if(hdop<3){                                              //Abbruch falls Genauigkeit zu schlecht ( unter 4 ist gut und über 8 ist schlecht)
            
              strncpy(UTC_h,Daten+7,2);                              //UTC parsen
              strncpy(UTC_m,Daten+9,2);
              strncpy(UTC_s,Daten+11,5);
              UTC_h[2]='\0';
              UTC_m[2]='\0';
              UTC_s[5]='\0';
            
              strncpy(LAT_grad,Daten+17,2);                          //Breitengrad parsen
              strncpy(LAT_decmin,Daten+19,7);
              LAT_grad[2]='\0';
              LAT_decmin[7]='\0';
            
              strncpy(N_S,Daten+27,1);                               //Norden oder Süden=?
              N_S[1]='\0';
            
            
              strncpy(LON_grad,Daten+29,3);                          //Längengrad parsen
              strncpy(LON_decmin,Daten+32,7);
              LON_grad[3]='\0';
              LON_decmin[7]='\0';
              
            
              strncpy(E_W,Daten+40,1);                               //Ostern oder Western?
              E_W[1]='\0';
    
             
             strncpy(SATANZ,Daten+44,2);                             //Satelitenanzahl parsen
             SATANZ[2]='\0';
             
             
             
              
             
              utc_h = atoi(UTC_h);                                   // Umwandlung der Stings in doubles
              //Serial.print("Zeit (GER/S): ");
              //Serial.print(utc_h+2,0);
              //Serial.print(":");
              utc_m = atoi(UTC_m);
              //Serial.print(utc_m,0);
              //Serial.print(":");
              utc_s = atof(UTC_s);
              //Serial.println(utc_s,0);
              lat_grad = atof(LAT_grad);
              //Serial.println(lat_grad);
              lat_decmin = atof(LAT_decmin);
              //Serial.println(lat_decmin,4);
              lon_grad = atof(LON_grad);
              //Serial.println(lon_grad);
              lon_decmin = atof(LON_decmin);
              //Serial.println(lon_decmin,4);
              
              if(N_S[0] == 'N'){                                     //N->1 und S->-1
                n_s = 1;
              }else{
                n_s = -1; 
              }
              
              if(E_W[0] == 'E'){                                     //E->1 und W->-1
                e_w = 1;
              }else{
                e_w = -1; 
              }
              
              satanz = atoi(SATANZ);  
           
              return 1;   
            }else return 0; 
  }
  
  
  void berechnung_kurswinkel_distanz()
  {
              lat = n_s*(lat_grad + lat_decmin/60)*PI/180; // Breitengrad in Bogenmaß
              lon = e_w*(lon_grad + lon_decmin/60)*PI/180; // Längengrad in Bogenmaß
              
              
              /*Serial.print("Akt. Position: ");           //Ausgabe der akt. Position auf dem Serial Monitor
              Serial.print(lat*180/PI,6);
              Serial.print(" ");
              Serial.println(lon*180/PI,6);*/
              
              /*delay(1000);                               //Ausgabe der akt. Position auf dem LSD-Display
              lcd.setCursor(0,0);
              lcd.print("                ");
              lcd.setCursor(0,1);
              lcd.print("                ");
              
              lcd.setCursor(0,0);
              lcd.print("Akt:");
              lcd.setCursor(5,0);
              lcd.print(lat*180/PI,6);
              lcd.setCursor(5,1);
              lcd.print(lon*180/PI,6);*/
              
              zentriw = 2.0*asin(sqrt(square(sin((lat-wegpunkte[zaehler][0])/2.0)) + cos(lat)*cos(wegpunkte[zaehler][0])*square(sin((wegpunkte[zaehler][1]-lon)/2.0)))); // in Bogenmaß!!
              distance = zentriw*6370000; // in m
              
              /*Serial.print("Ziel: ");                           //Ausgabe des nächsten Wegpunktes und des Abstands auf dem Serial Monitor
              Serial.print(wegpunkte[zaehler][0]*180/PI,6);
              Serial.print(" ");
              Serial.println(wegpunkte[zaehler][1]*180/PI,6);
              Serial.print("Abstand in m: ");
              Serial.println(distance);*/
              
                         
              kurswinkel = acos((sin(wegpunkte[zaehler][0])-sin(lat)*cos(zentriw))/(cos(lat)*sin(zentriw)));
                //Serial.print("Kurswinkel in Grad: ");
              
              if(lon <= wegpunkte[zaehler][1] && (wegpunkte[zaehler][1]-lon <= PI)){          //Fallunterscheidung für Lage des Ausgangspunktes 
                //Serial.println(kurswinkel*180/PI);
              }else if(lon > wegpunkte[zaehler][1] && (lon-wegpunkte[zaehler][1] <= PI)){
                kurswinkel = 2*PI-kurswinkel;
                //Serial.println(360 - (kurswinkel*180/PI));
              }else if(lon < wegpunkte[zaehler][1] && (wegpunkte[zaehler][1]-lon > PI)){
                kurswinkel = 2*PI-kurswinkel;
                //Serial.println(360 - kurswinkel*180/PI);
              }else if(lon > wegpunkte[zaehler][1] && (lon-wegpunkte[zaehler][1] > PI)){
                //Serial.println(kurswinkel*180/PI);
              }
  }
  
  
  void switch_wegpunkte()
  {
    
    //Timer für Button1 - Start
    if((digitalRead(buttonPin1) == HIGH) && (taste1 == LOW) && (done1 == 2)){
      start1=millis();
      taste1 = HIGH;
      done1 = 0;
    }
    
    //Timer für Button1 - gedrueckt
    if((digitalRead(buttonPin1) == HIGH) && (taste1 == HIGH))
    {
      drueck_zeit1 = millis()-start1;
    }
    
    
    if((digitalRead(buttonPin1) == LOW) && (taste1 == HIGH) && (drueck_zeit1 > ENTPRELLEN) && (drueck_zeit1 <= GEDRUECKT)){  
      
          if((zaehler-1)>=0){              //verringern bis Untergrenze erreicht, dann 6
            zaehler--;
          }else zaehler = 5;
          
      done1 = 1;
    }
    
    if((taste1 == HIGH) && (drueck_zeit1 > GEDRUECKT) && (done1 == 0)){
      
                lcd.setCursor(0,0);
                lcd.print("                ");
                lcd.setCursor(0,1);
                lcd.print("                ");
                
                lcd.setCursor(0,0);                    //Akt. Position ausgeben auf LCD
                lcd.print("Akt:");
                lcd.setCursor(5,0);
                lcd.print(lat*180/PI,6);
                lcd.setCursor(5,1);
                lcd.print(lon*180/PI,6);
                delay(3000);
                
      done1 = 1;
    }
    
    if((digitalRead(buttonPin1) == LOW) && (taste1 == HIGH) && (done1 == 1)){
      //Serial.print("FU");
      drueck_zeit1 = 0;
      done1 = 2;
      taste1 = LOW;
    }
    
    
    
    //Timer für Button2 - Start
    if((digitalRead(buttonPin2) == HIGH) && (taste2 == LOW) && (done2 == 2)){
      start2=millis();
      taste2 = HIGH;
      done2 = 0;
    }
    
    //Timer für Button2 - gedrueckt
    if((digitalRead(buttonPin2) == HIGH) && (taste2 == HIGH))
    {
      drueck_zeit2 = millis()-start2;
    }
    
    //Button2 kurz gerückt
    if((digitalRead(buttonPin2) == LOW) && (taste2 == HIGH) && (drueck_zeit2 > ENTPRELLEN) && (drueck_zeit2 <= GEDRUECKT)){    
      
          if(zaehler+1<6){                 //Erhöhen bis Obergrenze erreicht, dann wieder 0
            zaehler++;
          }else zaehler = 0;
          
      done2 = 1;
    }
    
    //Button2 lang gedrückt
    if((taste2 == HIGH) && (drueck_zeit2 > GEDRUECKT) && (done2 == 0)){
      
                lcd.setCursor(0,0);
                lcd.print("                ");
                lcd.setCursor(0,1);
                lcd.print("                ");
                
                lcd.setCursor(0,0);                          //Nächster WP anzeigen
                lcd.print("WP:");
                lcd.setCursor(5,0);
                lcd.print(wegpunkte[zaehler][0]*180/PI,6);
                lcd.setCursor(5,1);
                lcd.print(wegpunkte[zaehler][1]*180/PI,6);
                delay(3000);
                
      done2 = 1;
    }
    
    
    if((digitalRead(buttonPin2) == LOW) && (taste2 == HIGH) && (done2 == 1)){
      //Serial.print("FU");
      drueck_zeit2 = 0;
      done2 = 2;
      taste2 = LOW;
    }
  }