GPS: Winkel vom Standort zum Ziel berechnen

Hallo RN,
ich habe es nun geschafft GPS Daten auf mein Raspberry zu übertragen.

Mit diesen leicht angepassten Script (von kampis-elektroecke.de)

Code:

---

import serial
import sys
import time

Zeichen = 0
Laenge = 0
Qualitaet = 0
Satelliten = 0

Hoehe = 0.0
Breitengrad = 0.0
Laengengrad = 0.0

Input = ""
Uhrzeit = ""
Checksumme = ""

Datenliste = []

# UART oeffnen
UART = serial.Serial("/dev/ttyAMA0 ", 38400)
UART.open()

# Startmeldung
print ""
print "+---------------------------------------------------------------------+"
print "| Dieses Programm empfaengt Daten einer Holux GPS-Maus,              |"
print "| die mittels GAA-Protokoll uebertragen wurden.                      |"
print "| Der empfangene Datensatz wird zerlegt und in der Konsole angezeigt. |"
print "+---------------------------------------------------------------------+"
print ""

while True:

        Zeichen = 0

        # String leeren
        Input = ""
       
        # Zeichen empfangen
        Zeichen = UART.read()
       
        # Pruefen ob Uebertragung gestartet wurde
        if Zeichen == "$":

                # Zeichen 2-6 einlesen
                for Counter in range(4):

                        Zeichen = 0
                        Zeichen = UART.read()
                        Input = Input + str(Zeichen)
       
                # Pruefen ob das GGA Protokoll gesendet wird
                if Input == "GPGG":

                        # Zeichen empfangen bis ein LF als Abschluss kommt
                        while Zeichen != "\n":
                                Zeichen = 0
                                Zeichen = UART.read()
                                Input = Input + str(Zeichen)
                               
                        Input = Input.replace("\r\n", "")
       
                        # Datensatz nach jedem "," trennen und in einer Liste speichern
                        Datenliste = Input.split(",")

                        # Laenge des Datensatzes feststellen
                        Laenge = len(Input)

                        # Uhrzeit herausfiltern
                        Uhrzeit = Datenliste[1]
                        Uhrzeit = Uhrzeit[0:2] + ":" + Uhrzeit[2:4] + ":" + Uhrzeit[4:6]

                        # Laengen und Breitengrad herausfiltern und berechnen
                        Breitengrad = float(Datenliste[2])
                        Breitengrad = Breitengrad / 100

                        Laengengrad = float(Datenliste[4])
                        Laengengrad = Laengengrad / 100

                        # Signalqualitaet herausfiltern
                        Qualitaet = int(Datenliste[6])

                        # Anzahl der Satelliten herausfiltern
                        Satelliten = int(Datenliste[7])

                        # Hoehe herausfiltern
                        Hoehe = float(Datenliste[9])
                       
                        # Checksumme auslesen
                        Checksumme = Datenliste[14]

                        # Ausgabe
                        print Input
                        print ""
                        print "Laenge des Datensatzes:", Laenge, "Zeichen"
                        print "Uhrzeit:", Uhrzeit
                        print "Breitengrad:", Breitengrad, Datenliste[3]
                        print "Laengengrad:", Laengengrad, Datenliste[5]
                        print "Hoehe ueber dem Meeresspiegel:", Hoehe, Datenliste[10]
                        print "GPS-Qualitaet:", Qualitaet
                        print "Anzahl der Satelliten:", Satelliten
                        print "Checksumme:", Checksumme
                        print "-------------------------------------------"

---

werden mir die GPS Daten angezeigt.
Das "gleiche" habe ich auf meinem RP6 schon geschafft.

Nun geht es darum den Winkel vom Standort zum Ziel (Vorgebender GPS Punkt) zu berechnen.

Code:

---

/*
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples
 * ****************************************************************************
 * Example: NL-552ETTL
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Now we will use the UART to read data from the NL-552ETTL GPS module
 * (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS chipset.
 *
 * The NL-552ETTL TX pin (4) has to be connected to RP6 CONTROL pin RXD
 * (PROG_UART: 2). The internal pullup resistor of RXD should be
 * deactivated!
 *
 * ****************************************************************************
 * NMEA 0183 short description:
 * 1. GPRMC:
 * $GPRMC,213938.000,A,5108.3356,N,00951.0774,E,0.00,79.64,061012,,,A*53
 * Index: 1          2 3        4 5          6 7    8    9        12CS
 *  1    = UTC of position [hhmmss.sss]
 *  2    = Data status (A=ok, V=void)
 *  3    = Latitude of fix [ddmm.mmmm]
 *  4    = N or S
 *  5    = Longitude of fix [dddmm.mmmm]
 *  6    = E or W
 *  7    = Speed over ground in knots
 *  8    = Track made good, degrees true
 *  9    = UT date [ddmmyy]
 *  10  = Magnetic variation degrees (Easterly var. subtracts from true course)
 *  11  = E or W
 *  12  = Mode indicator (NMEA V2.3):
 *          A=Autonomous mode
 *          D=Differential mode
 *          E=Estimated (dead-reckoning) mode
 *          M=Manual input mode
 *          S=Simulated mode
 *          N=Data not valid
 *  CS  = Checksum
 *
 * 2. GPVTG:
 * $GPVTG,79.64,T,,M,0.00,N,0.0,K,A*31
 * Index: 1    2  4 5    6 7  8 9 CS
 *  1    = Track made good
 *  2    = Fixed text 'T' indicates that track made good is relative to true north
 *  3    = Track degrees
 *  4    = M=Magnetic
 *  5    = Speed over ground in knots
 *  6    = Fixed text 'N' indicates that speed over ground is in knots
 *  7    = Speed over ground in kilometers/hour
 *  8    = Fixed text 'K' indicates that speed over ground is in kilometers/hour
 *  9    = Mode indicator (A,D,E,M,S,N)
 *  CS  = Checksum
 *
 * 3. GPGGA:
 * $GPGGA,213938.000,5108.3356,N,00951.0774,E,1,04,5.3,158.5,M,46.2,M,,0000*52
 * Index: 1          2        3 4          5 6 7  8  9      11  12 14  CS
 *  1    = UTC of position
 *  2    = Latitude of fix
 *  3    = N or S
 *  4    = Longitude of fix
 *  5    = E or W
 *  6    = GPS quality indicator:
 *          0=invalid
 *          1=GPS fix (SPS)
 *          2=DGPS fix
 *          3=PPS fix
 *          4=Real Time Kinematic
 *          5=Float RTK
 *          6=estimated (dead reckoning) (NMEA V2.3)
 *          7=Manual input mode
 *          8=Simulation mode
 *  7    = Number of satellites in use [not those in view]
 *  8    = Horizontal dilution of position (HDOP)
 *  9    = Antenna altitude above/below mean sea level (geoid)
 *  10  = Meters  (Antenna height unit)
 *  11  = Geoidal separation (Diff. between WGS-84 earth ellipsoid and
 *          mean sea level.  -=geoid is below WGS-84 ellipsoid)
 *  12  = Meters  (Units of geoidal separation)
 *  13  = Age in seconds since last update from diff. reference station
 *  14  = Differential reference station ID#
 *  CS  = Checksum
 *
 * 4. GPGSA:
 * $GPGSA,A,3,02,04,10,07,,,,,,,,,7.4,5.3,5.1*33
 * Index: 1 2 3  4  5  6          15  16  17  CS
 *  1    = Selection mode:
 *          M=Manual, forced to operate in 2D or 3D
 *          A=Automatic 2D/3D
 *  2    = Mode:
 *          1=no fix
 *          2=2D
 *          3=3D
 *  3-14 = IDs of SVs used in position fix (null for unused fields)
 *  15  = PDOP
 *  16  = HDOP
 *  17  = VDOP
 *  CS  = Checksum
 *
 * 5. GPGSV:
 * $GPGSV,3,1,12,02,36,301,37,04,53,235,21,05,03,294,,07,35,170,29*79
 * $GPGSV,3,2,12,08,04,187,28,10,56,290,36,13,88,106,20,16,12,076,*7D
 * $GPGSV,3,3,12,20,23,118,11,23,50,067,,29,03,351,12,30,16,048,22*70
 * Index: 1 2 3  4  5  6  7  8  9  10  12 13 14  15 16 17 18  19 CS
 *  1    = Total number of messages of this type in this cycle
 *  2    = Message number
 *  3    = Total number of SVs in view
 *  4    = SV PRN number
 *  5    = Elevation in degrees, 90 maximum
 *  6    = Azimuth, degrees from true north, 000 to 359
 *  7    = SNR, 00-99 dB (null when not tracking)
 *  8-11 = Information about second SV, same as field 4-7
 *  12-15= Information about third SV, same as field 4-7
 *  16-19= Information about fourth SV, same as field 4-7
 *  CS  = Checksum
 *
 * 6. GPGLL:
 * $GPGLL,5108.3356,N,00951.0774,E,213938.000,A,A*5C
 * Index: 1        2 3          4 5          6 7 CS
 *  1    = Latitude of fix
 *  2    = N or S
 *  3    = Longitude of fix
 *  4    = E or W
 *  5    = UTC of position
 *  6    = Data status (A=ok, V=void)
 *  7    = Mode indicator (A,D,E,M,S,N)
 *  CS  = Checksum
 *
 * COMMENTS:
 * a) Each sentence (line) begins with '$'.
 * b) Each sentence (line) ends with '*', followed by the checksum (two
 *    chars building an 8-bit HEX number) and terminated by <CR><LF>.
 * c) The checksum (CS) is calculated by XOR of all chars between '$' and '*'.
 * d) The max. sentence (line) length is 82 chars (including <CR><LF>).
 * e) Further infos:
 *    -> http://www.nmea.org/
 *    -> http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm
 *    -> http://de.wikipedia.org/wiki/NMEA_0183
 *    -> http://aprs.gids.nl/nmea/
 *
 * ############################################################################
 * The Robot does NOT move in this example! You can simply put it on a table
 * next to your PC and you should connect it to the PC via the USB Interface!
 * ############################################################################
 * ****************************************************************************
 */
/*****************************************************************************/
// Includes:
#include "RP6ControlLib.h"
#include <stdint.h>
        // Always needs to be included!

/*****************************************************************************/
// NL-552ETTL functions:
char parseBuffer[256];
uint16_t parseBuffer_length = 0;
uint8_t parseBuffer_pos = 0;

/**
 * Clears the parseBuffer
 */
void clearParseBuffer(void)
{
  parseBuffer_pos = 0;
  parseBuffer_length = 0;
}

/**
 * Writes received byte data into buffer parseBuffer and returns if a full
 * NMEA sentence has been received. Then the buffer is terminated with 0.
 * Returns 0 if a sentence has been detected and buffer is ready.
 * Returns 1 if no sentence has been received yet
 * Returns 2 if buffer overflow occured
 */
uint8_t parseNMEAsentence(char *sentence, uint8_t data)
{
  static uint8_t startflag = false;
  static char last_data[10];
  uint16_t length = 0;
  uint8_t i = 0;

  if(data == 0)
    return 1;

  while(sentence[i++])
    length++;

  if(length > 10)
    length = 10;

  for(i = 0; i < 9; i++)
    last_data[i] = last_data[i + 1];

  last_data[9] = data;

  parseBuffer[parseBuffer_pos] = data;
  parseBuffer_pos++;
  parseBuffer_length++;

  if(parseBuffer_pos > 254)
  {
    clearParseBuffer();
    startflag = false;
    return 2;
  }

  if(!startflag && (strstr(last_data, sentence) != NULL))
  { // Chosen sentence start
    for(i = 0; i < length; i++)
      parseBuffer[i] = sentence[i];

    parseBuffer_pos = length;
    parseBuffer_length = length;
    startflag = true;
  }

  if(startflag)
  {
    if(data == '\r')
    {    // Chosen sentence end
      parseBuffer[parseBuffer_pos] = 0;
      last_data[9] = 0;
      startflag = false;
      return 0;
    }
  }
  return 1;
}

/**
 * Outputs true, if the NMEA sentence in parseBuffer is intact,
 * else returns false.
 */
uint8_t checkNMEAsentence(void)
{
  uint8_t i, cs;
  char cs_str[3];

  if(parseBuffer_length < 18) return false;
  if(parseBuffer_length > 82) return false;
  if(parseBuffer[0] != '$') return false;
  if(parseBuffer[parseBuffer_length - 4] != '*') return false;

  cs = parseBuffer[1];
  for(i = 2; i < (parseBuffer_length - 4); i++)
  {
    cs ^= parseBuffer[i];  // Calculate checksum
  }

  itoa(cs, &cs_str[0], HEX);
  strupr(&cs_str[0]);    // cs as uppercase chars
  if((cs_str[0] != parseBuffer[parseBuffer_length - 3])
  || (cs_str[1] != parseBuffer[parseBuffer_length - 2])) return false;

  return true;
}
/**
 * Outputs a result string containing the data with index of the NMEA
 * sentence in parseBuffer.
 */
void parseNMEAitem(char *result, uint8_t index)
{
  uint8_t index_cnt, buffer_idx, result_idx;

  index_cnt = index;
  buffer_idx = 6;

  while(index_cnt)
  {
    if(parseBuffer[buffer_idx++] == ',')
      index_cnt--;
  }

  result_idx = 0;

  while((parseBuffer[buffer_idx] != ',')
    && (parseBuffer[buffer_idx] != '*'))
  {
    result[result_idx++] = parseBuffer[buffer_idx++];
  }

  result[result_idx] = 0;
}

typedef struct
{
  int32_t latitude;
  int32_t longitude;
} geographic_t;

/* mapping of location difference ratios to the range [0; 45[ degrees: */
static const int16_t ATAN[45] = {
    8,  26,  43,  61,  78,  96, 113, 131, 149, 167,
  185, 203, 221, 240, 258, 277, 296, 315, 334, 354,
  373, 393, 414, 434, 455, 476, 498, 520, 542, 565,
  589, 612, 637, 661, 687, 713, 739, 767, 795, 824,
  854, 884, 916, 948, 982 };

/* the destination coordinates times 1000 */
static const geographic_t DESTINATION = {52011, 11686}; //schönebeck start

/* convert location difference ratio to an angle.
 *
 *  ratio -- ratio of location differences
 *
 *  returns angle in degrees
 */
static int16_t ratio2angle(int32_t ratio)
{
  uint8_t i;

  for(i=0; i<45; i++)
  {
    if(ratio < ATAN[i])
      return i;
  }

  return 45;
}

/* get the direction to the destination
 *
 *  current -- current location
 *
 *  returns course angle in degrees
 *  0    <-> N
 *  90  <-> W
 *  180  <-> S
 *  270  <-> E
 *  -1  <-> destination reached
 */
static int16_t get_direction(geographic_t * current)
{
  int32_t d_latitude, d_longitude, ratio;

  /* compute the coordinate differences: */
  d_latitude = DESTINATION.latitude - current->latitude;
  d_longitude = DESTINATION.longitude - current->longitude;

  /* compute the course depending on the 8th we are in: */
  if(d_longitude < 0) // 2nd or 3rd quadrant
  {
    d_longitude = -d_longitude;

    if(d_latitude < 0) // 3rd quadrant
    {
      d_latitude = -d_latitude;

      if(d_longitude > d_latitude) // upper 8th
      {
        ratio = (d_latitude*1000)/d_longitude;
        return 90 + ratio2angle(ratio);
      }
      else // lower 8th
      {
        ratio = (d_longitude*1000)/d_latitude;
        return 180 - ratio2angle(ratio);
      }

    }
    else // 2nd quadrant
    {
      if(d_longitude > d_latitude) // lower 8th
      {
        ratio = (d_latitude*1000)/d_longitude;
        return 90 - ratio2angle(ratio);
      }
      else // upper 8th
      {
        ratio = (d_longitude*1000)/d_latitude;
        return ratio2angle(ratio);
      }
    }
  }
  else // 1st or 4th quadrant
  {
    if(d_latitude < 0) // 4th_quadrant
    {
      d_latitude = -d_latitude;

      if(d_longitude > d_latitude) // upper 8th
      {
        ratio = (d_latitude*1000)/d_longitude;
        return 270 - ratio2angle(ratio);
      }
      else // lower 8th
      {
        ratio = (d_longitude*1000)/d_latitude;
        return 180 + ratio2angle(ratio);
      }
    }
    else // 1st quadrant
    {
      if(d_longitude > d_latitude) // lower 8th
      {
        ratio = (d_latitude*1000)/d_longitude;
        return 270+ratio2angle(ratio);
      }
      else if(d_latitude) // upper 8th
      {
        ratio = (d_longitude*1000)/d_latitude;
        return 360 - ratio2angle(ratio);
      }
      else
      {
        // we are at the destination:
        return -1;
      }
    }
  }
}

/* convert latitude coordinate string to degrees
 *
 *  latitude -- string with format ddmm.mmmm
 *
 *  return degrees * 1000 as integer
 */
static int32_t latitude2int(const char * latitude)
{ 
  int32_t degrees, minutes;   

  degrees  = (int32_t)(latitude[0] - '0') * 10000; 
  degrees += (latitude[1] - '0') * 1000;

  minutes  = (int32_t)(latitude[2] - '0') * 10000;
  minutes += (latitude[3] - '0') * 1000;
  minutes += (latitude[5] - '0') * 100; 
  minutes += (latitude[6] - '0') * 10; 
  minutes += (latitude[7] - '0'); 
  degrees += minutes / 60;

  return degrees;
}

/* convert longitude coordinate string to degrees
 *
 *  latitude -- string with format dddmm.mmmm
 *
 *  return degrees * 1000 as integer
 */
static int32_t longitude2int(const char * longitude)
{
  int32_t degrees, minutes;

  degrees  = (int32_t)(longitude[0] - '0') * 100000;
  degrees += (int32_t)(longitude[1] - '0') * 10000;
  degrees += (longitude[2] - '0') * 1000;

  minutes  = (int32_t)(longitude[3] - '0') * 10000;
  minutes += (int32_t)(longitude[4] - '0') * 1000;
  minutes += (longitude[6] - '0') * 100;
  minutes += (longitude[7] - '0') * 10;
  minutes += (longitude[8] - '0');

  degrees += minutes / 60;

  return degrees;
}

/* convert string coordinates to geographic coordinates
 *
 *  latitude  -- string with format dddmm.mmmm
 *  longitude  -- string with format ddmm.mmmm
 *  coordinate -- pointer to geographic type for output
 */
static void convert_coordinates(const char * latitude, const char * longitude, geographic_t * coordinate)
{
  coordinate->latitude = latitude2int(latitude);
  coordinate->longitude = longitude2int(longitude);
}

/**
 * Write a floating point number to the LCD.
 *
 * Example:
 *
 *  // Write a floating point number to the LCD (no exponent):
 *  writeDoubleLCD(1234567.890, 11, 3);
 *
 * The value of prec (precision) defines the number of decimal places.
 * For 32 bit floating point variables (float, double ...) 6 is
 * the max. value for prec (7 relevant digits).
 * The value of width defines the overall number of characters in the
 * floating point number including the decimal point. The number of
 * pre-decimal positions is: (width - prec - 1).
 */
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
 dtostrf(number, width, prec, &buffer[0]);
 writeStringLCD(&buffer[0]);
}


/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
  /* init controller and LC-Display: */
  initRP6Control();
  initLCD();

  /* blink LEDs and show startup message: */
  setLEDs(0b1111);
  mSleep(500);
  setLEDs(0b0000);
  // Write a text message to the LCD:
  showScreenLCD("######GPS#######", "################");
  mSleep(1500);

  clearLCD();


  while(true) // main program loop
  {
    uint8_t ch;

    /* clear buffers */
    clearParseBuffer();
    clearReceptionBuffer();

    /* receive data from GPS module: */
    do
    {
      ch = readChar();
    } while(parseNMEAsentence("$GPRMC", ch));

    if(checkNMEAsentence()) // data ok?
    {
      char lat[12];
      char lon[12];
      char item[12];
      int16_t course_angle;
      geographic_t current={0};

      /* extract latitude and longitude: */
      strcpy(lat, "0211.1111");
      strcpy(lon, "11135.5000");

      /* convert to geographic type and compute course angle: */
      convert_coordinates(lat, lon, &current);
 
      course_angle = get_direction(&current);

      /* correct 360° course */
      if(course_angle == 360)
        course_angle = 0;

    /* prepare coordinates for display: */
    if(lat[0] == '0') // leading zero?   
    {
      lat[0] = lat[1]; 
      lat[1] = '°';     
      lat[4] = '\''; 
      lat[5] = 0;   
    }
    else
    {
        lat[4] = lat[3];
        lat[3] = lat[2];
        lat[2] = '°';
        lat[5] = '\'';
        lat[6] = 0;
      }

      if(lon[0] == '0') // leading zero?
      {
        if(lon[1] == '0') // two leading zeros?
        {
          lon[0] = lon[2];
          lon[1] = '°';
          lon[2] = lon[3];
          lon[3] = lon[4];
          lon[4] = '\'';
          lon[5] = 0;
        }
        else
        {
          lon[0] = lon[1];
          lon[1] = lon[2];
          lon[2] = '°';
          lon[5] = '\'';
          lon[6] = 0;
        }
      }
      else
      {
        lon[4] = lon[3];
        lon[3] = lon[2];
        lon[2] = '°';
        lon[5] = '\'';
        lon[6] = 0;
      }

      /* show coordinates: */
      clearLCD();
      setCursorPosLCD(0, 0);  // line 1
          parseNMEAitem(lat, 3);
      writeStringLCD("Lat:");  writeStringLCD(lat);

      setCursorPosLCD(1, 0);  // line 2
      parseNMEAitem(lon, 5);
      writeStringLCD("Lon:");  writeStringLCD(lon);

      mSleep(1000);

      clearLCD();

      setCursorPosLCD(0, 0);  // line 1
      parseNMEAitem(item, 7);
      writeStringLCD("Speed:");  writeStringLCD(item); writeStringLCD(" KN");

      /* avoid float calculations: */
      item[2] = item[3];
      item[3] = 0;

      int32_t a = atoi(item);

      a = a * 1852;
      a = a / 10000;
      setCursorPosLCD(1, 0);  // line 1

      writeStringLCD("Speed:"); writeIntegerLCD(a, DEC); writeStringLCD(" KM/H");
      mSleep(1000);

      clearLCD();

      setCursorPosLCD(0, 0);  // line 1

      if(course_angle < 0)
      {
        writeStringLCD("Ziel erreicht!");
        mSleep(2000);
        clearLCD();
      }
      else
      {
        writeStringLCD("Ziel:");  writeIntegerLCD(course_angle, DEC);
        mSleep(2000);
        clearLCD();
      }
    }
  }
  return 0;
}

---

Dieser Script war für meinen RP6 angepasst. Kann mir wer dabei helfen in für den PI klar zu machen?


LG

Hey,

ich weiß nicht wie vertraut du mit der Vektorrechnung bist, aber ich glaube die wäre ein guter Ansatz.
Zum Verständnis:
Du hast zwei Punkte:
1. Zielpunkt
2. Den Punkt des Robos (GPS Modul)

Das GPS Modul deines Robos ist der Ursprung deines Koordinatensystems und mit Hilfe dieses Moduls kannst du den Differenzenvektor zwischen Ziel und Robo bestimmen.
Der Vektor sieht dann so aus:

r = Längengrad, Breitengrad

Du hast also einen Vektor der die Koordinaten des Ziels beschreibt und die Koordinaten deines GPS Moduls als Vektor, die den Nullpunkt darstellen.
Jetzt kannst du den Differenzenvektor berechnen: Differenzenvektor = Zielvektor - Koordinatenvektor deines Robos
Der Differenzenvektor beschreibt dann den Weg, den dein Robo noch gehen muss um zum Ziel zu kommen (also die Koordinatenentfernung in x und y Richtung).
Mit Hilfe dieser Koordinaten kannst du nun die Winkel zwischen x und y Achse ausrechnen und herausfinden in welchem Winkel der Robo zum Ziel steht.
Wenn du den Betrag beider Koordinaten ausrechnest, bekommst du die direkte Entfernung raus.

Weiß nicht ob es so klappt, aber das hört sich nach einer Aufgabe an, die ideal für Vektoren ist ;)