• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

GPS_NMEA.cpp

Go to the documentation of this file.
00001 /*
00002         GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
00003         Code by Jordi Muņoz and Jose Julio. DIYDrones.com
00004         This code works with boards based on ATMega168/328 and ATMega1280/2560 (Serial port 1)
00005 
00006         This library is free software; you can redistribute it and/or
00007     modify it under the terms of the GNU Lesser General Public
00008     License as published by the Free Software Foundation; either
00009     version 2.1 of the License, or (at your option) any later version.
00010 
00011         GPS configuration : NMEA protocol
00012         Baud rate : 38400
00013         NMEA Sentences : 
00014                 $GPGGA : Global Positioning System Fix Data
00015                 $GPVTG : Ttack and Ground Speed
00016                 
00017         Methods:
00018                 Init() : GPS Initialization
00019                 Read() : Call this funcion as often as you want to ensure you read the incomming gps data
00020                 
00021         Properties:
00022                 Lattitude : Lattitude * 10000000 (long value)
00023                 Longitude : Longitude * 10000000 (long value)
00024                 Altitude :  Altitude * 1000 (milimeters) (long value)
00025                 Ground_speed : Speed (m/s) * 100 (long value)
00026                 Ground_course : Course (degrees) * 100 (long value)
00027                 Type : 2 (This indicate that we are using the Generic NMEA library)
00028                 NewData : 1 when a new data is received.
00029                           You need to write a 0 to NewData when you read the data
00030                 Fix : >=1: GPS FIX, 0: No Fix (normal logic)
00031                 Quality : 0 = No Fix
00032                            1 = Bad (Num sats < 5)
00033                                    2 = Poor
00034                                    3 = Medium
00035                                    4 = Good
00036 
00037    NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
00038 */
00039 
00040 #include "GPS_NMEA.h"
00041 
00042 #include <avr/interrupt.h>
00043 #include "WProgram.h"
00044 
00045 
00046 // Constructors ////////////////////////////////////////////////////////////////
00047 GPS_NMEA_Class::GPS_NMEA_Class()
00048 {
00049 }
00050 
00051 // Public Methods //////////////////////////////////////////////////////////////
00052 void GPS_NMEA_Class::Init(void)
00053 {
00054         Type = 2;
00055         GPS_checksum_calc = false;
00056         bufferidx = 0;
00057         NewData=0;
00058         Fix=0;
00059         Quality=0;
00060         PrintErrors=0;  
00061         // Initialize serial port
00062         #if defined(__AVR_ATmega1280__)|| defined(__AVR_ATmega2560__)
00063                 Serial1.begin(38400);         // Serial port 1 on ATMega1280/2560
00064         #else
00065                 Serial.begin(38400);
00066         #endif
00067 }
00068 
00069 // This code donīt wait for data, only proccess the data available on serial port
00070 // We can call this function on the main loop (50Hz loop)
00071 // If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
00072 void GPS_NMEA_Class::Read(void)
00073 {
00074   char c;
00075   int numc;
00076   int i;
00077  
00078   
00079   #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)    // If AtMega1280/2560 then Serial port 1...
00080         numc = Serial1.available();    
00081   #else
00082         numc = Serial.available();
00083   #endif
00084   if (numc > 0)
00085     for (i=0;i<numc;i++){
00086       #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)    // If AtMega1280/2560 then Serial port 1...
00087           c = Serial1.read();   
00088       #else
00089           c = Serial.read();
00090       #endif
00091       if (c == '$'){                      // NMEA Start
00092         bufferidx = 0;
00093         buffer[bufferidx++] = c;
00094         GPS_checksum = 0;
00095         GPS_checksum_calc = true;
00096         continue;
00097         }
00098       if (c == '\r'){                     // NMEA End
00099         buffer[bufferidx++] = 0;
00100                 parse_nmea_gps();
00101         }
00102       else {
00103         if (bufferidx < (GPS_BUFFERSIZE-1)){
00104           if (c == '*')
00105             GPS_checksum_calc = false;    // Checksum calculation end
00106           buffer[bufferidx++] = c;
00107           if (GPS_checksum_calc)
00108             GPS_checksum ^= c;            // XOR 
00109           }
00110                 else
00111                   bufferidx=0;   // Buffer overflow : restart
00112         }
00113     }   
00114 }
00115 
00116 /****************************************************************
00117  * 
00118  ****************************************************************/
00119 // Private Methods //////////////////////////////////////////////////////////////
00120 void GPS_NMEA_Class::parse_nmea_gps(void)
00121 {
00122   byte NMEA_check;
00123   long aux_deg;
00124   long aux_min;
00125   char *parseptr;
00126 
00127   
00128   if (strncmp(buffer,"$GPGGA",6)==0){        // Check if sentence begins with $GPGGA
00129     if (buffer[bufferidx-4]=='*'){           // Check for the "*" character
00130       NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]);    // Read the checksums characters
00131       if (GPS_checksum == NMEA_check){      // Checksum validation
00132         //Serial.println("buffer");
00133                 NewData = 1;  // New GPS Data
00134         parseptr = strchr(buffer, ',')+1;
00135         //parseptr = strchr(parseptr, ',')+1;
00136                 Time = parsenumber(parseptr,2);          // GPS UTC time hhmmss.ss
00137                 parseptr = strchr(parseptr, ',')+1;
00138                 //
00139         aux_deg = parsedecimal(parseptr,2);      // degrees
00140         aux_min = parsenumber(parseptr+2,4);     // minutes (sexagesimal) => Convert to decimal
00141         Lattitude = aux_deg*10000000 + (aux_min*50)/3;   // degrees + minutes/0.6  (*10000000) (0.6 = 3/5)
00142         parseptr = strchr(parseptr, ',')+1;
00143                 //
00144                 if (*parseptr=='S')
00145                   Lattitude = -1*Lattitude;              // South Lattitudes are negative
00146                 //
00147         parseptr = strchr(parseptr, ',')+1;
00148         // W Longitudes are Negative
00149         aux_deg = parsedecimal(parseptr,3);      // degrees
00150         aux_min = parsenumber(parseptr+3,4);     // minutes (sexagesimal)
00151         Longitude = aux_deg*10000000 + (aux_min*50)/3;  // degrees + minutes/0.6 (*10000000)
00152         //Longitude = -1*Longitude;                   // This Assumes that we are in W longitudes...
00153         parseptr = strchr(parseptr, ',')+1;
00154                 //
00155                 if (*parseptr=='W')
00156                   Longitude = -1*Longitude;              // West Longitudes are negative
00157                 //
00158         parseptr = strchr(parseptr, ',')+1;
00159         Fix = parsedecimal(parseptr,1);
00160         parseptr = strchr(parseptr, ',')+1;
00161         NumSats = parsedecimal(parseptr,2);
00162         parseptr = strchr(parseptr, ',')+1; 
00163         HDOP = parsenumber(parseptr,1);          // HDOP * 10
00164         parseptr = strchr(parseptr, ',')+1;
00165         Altitude = parsenumber(parseptr,1)*100;  // Altitude in decimeters*100 = milimeters
00166                 if (Fix < 1)
00167                   Quality = 0;      // No FIX
00168                 else if(NumSats<5)
00169                   Quality = 1;      // Bad (Num sats < 5)
00170                 else if(HDOP>30)
00171                   Quality = 2;      // Poor (HDOP > 30)
00172                 else if(HDOP>25)
00173                   Quality = 3;      // Medium (HDOP > 25)
00174                 else
00175                   Quality = 4;      // Good (HDOP < 25)
00176         }
00177           else
00178             {
00179                 if (PrintErrors)
00180               Serial.println("GPSERR: Checksum error!!");
00181             }
00182       }
00183     }
00184   else if (strncmp(buffer,"$GPVTG",6)==0){        // Check if sentence begins with $GPVTG
00185     //Serial.println(buffer);
00186     if (buffer[bufferidx-4]=='*'){           // Check for the "*" character
00187       NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]);    // Read the checksums characters
00188       if (GPS_checksum == NMEA_check){      // Checksum validation
00189         parseptr = strchr(buffer, ',')+1;
00190         Ground_Course = parsenumber(parseptr,2);      // Ground course in degrees * 100
00191         parseptr = strchr(parseptr, ',')+1;
00192         parseptr = strchr(parseptr, ',')+1;
00193         parseptr = strchr(parseptr, ',')+1;
00194         parseptr = strchr(parseptr, ',')+1;
00195         parseptr = strchr(parseptr, ',')+1;
00196         parseptr = strchr(parseptr, ',')+1;
00197         Ground_Speed = parsenumber(parseptr,2)*10/36; // Convert Km/h to m/s (*100)
00198         //GPS_line = true;
00199         }
00200           else
00201             {
00202                 if (PrintErrors)
00203               Serial.println("GPSERR: Checksum error!!");
00204             }
00205     }
00206   }
00207   else
00208     {
00209         bufferidx = 0;
00210         if (PrintErrors)
00211           Serial.println("GPSERR: Bad sentence!!");
00212     }
00213 }
00214 
00215 
00216 /****************************************************************
00217  * 
00218  ****************************************************************/
00219  // Parse hexadecimal numbers
00220 byte GPS_NMEA_Class::parseHex(char c) {
00221     if (c < '0')
00222       return (0);
00223     if (c <= '9')
00224       return (c - '0');
00225     if (c < 'A')
00226        return (0);
00227     if (c <= 'F')
00228        return ((c - 'A')+10);
00229 }
00230 
00231 // Decimal number parser
00232 long GPS_NMEA_Class::parsedecimal(char *str,byte num_car) {
00233   long d = 0;
00234   byte i;
00235   
00236   i = num_car;
00237   while ((str[0] != 0)&&(i>0)) {
00238      if ((str[0] > '9') || (str[0] < '0'))
00239        return d;
00240      d *= 10;
00241      d += str[0] - '0';
00242      str++;
00243      i--;
00244      }
00245   return d;
00246 }
00247 
00248 // Function to parse fixed point numbers (numdec=number of decimals)
00249 long GPS_NMEA_Class::parsenumber(char *str,byte numdec) {
00250   long d = 0;
00251   byte ndec = 0;
00252   
00253   while (str[0] != 0) {
00254      if (str[0] == '.'){
00255        ndec = 1;
00256      }
00257      else {
00258        if ((str[0] > '9') || (str[0] < '0'))
00259          return d;
00260        d *= 10;
00261        d += str[0] - '0';
00262        if (ndec > 0)
00263          ndec++;
00264        if (ndec > numdec)   // we reach the number of decimals...
00265          return d;
00266        }
00267      str++;
00268      }
00269   return d;
00270 }
00271 
00272 GPS_NMEA_Class GPS; 

Generated for ArduPilot Libraries by doxygen