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

/home/jgoppert/Projects/ap/libraries/AP_GPS/AP_GPS_NMEA.cpp

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

Generated for ArduPilot Libraries by doxygen