ardupilot/libraries/AP_GPS/AP_GPS_NMEA.cpp

245 lines
7.4 KiB
C++
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
Edits by HappyKillmore
This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1)
This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
GPS configuration : NMEA protocol
Baud rate : 38400
NMEA Sentences :
$GPGGA : Global Positioning System fix Data
$GPVTG : Ttack and Ground Speed
Methods:
init() : GPS Initialization
read() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
latitude : latitude * 10000000 (long value)
longitude : longitude * 10000000 (long value)
altitude : altitude * 1000 (milimeters) (long value)
ground_speed : Speed (m / s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value)
Type : 2 (This indicate that we are using the Generic NMEA library)
new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data
fix : > = 1: GPS FIX, 0: No fix (normal logic)
quality : 0 = No fix
1 = Bad (Num sats < 5)
2 = Poor
3 = Medium
4 = Good
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
*/
#include "AP_GPS_NMEA.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::init(void)
{
//Type = 2;
// initialize serial port for binary protocol use
_port->print(NMEA_OUTPUT_SENTENCES);
_port->print(NMEA_OUTPUT_4HZ);
_port->print(SBAS_ON);
_port->print(DGPS_SBAS);
_port->print(DATUM_GOOGLE);
}
// This code don<6F>t wait for data, only proccess the data available on serial port
// We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
bool
AP_GPS_NMEA::read(void)
{
char c;
int numc;
int i;
bool parsed = false;
numc = _port->available();
if (numc > 0){
for (i = 0; i < numc; i++){
c = _port->read();
if (c == '$'){ // NMEA Start
bufferidx = 0;
buffer[bufferidx++] = c;
GPS_checksum = 0;
GPS_checksum_calc = true;
continue;
}
if (c == '\r'){ // NMEA End
buffer[bufferidx++] = 0;
parsed = parse_nmea_gps();
} else {
if (bufferidx < (GPS_BUFFERSIZE - 1)){
if (c == '*')
GPS_checksum_calc = false; // Checksum calculation end
buffer[bufferidx++] = c;
if (GPS_checksum_calc){
GPS_checksum ^= c; // XOR
}
} else {
bufferidx = 0; // Buffer overflow : restart
}
}
}
}
}
/****************************************************************
*
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
// Private Methods //////////////////////////////////////////////////////////////
bool
AP_GPS_NMEA::parse_nmea_gps(void)
{
uint8_t NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer");
parseptr = strchr(buffer, ',')+1;
//parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ',')+1;
aux_deg = parsedecimal(parseptr, 2); // degrees
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal
latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ',')+1;
// W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ',')+1;
fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ',')+1;
num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ',')+1;
hdop = parsenumber(parseptr, 1); // HDOP * 10
parseptr = strchr(parseptr, ',')+1;
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
if (fix < 1)
quality = 0; // No FIX
else if(num_sats < 5)
quality = 1; // Bad (Num sats < 5)
else if(hdop > 30)
quality = 2; // Poor (HDOP > 30)
else if(hdop > 25)
quality = 3; // Medium (HDOP > 25)
else
quality = 4; // Good (HDOP < 25)
} else {
_error("GPSERR: Checksum error!!\n");
return false;
}
}
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
parseptr = strchr(buffer, ',')+1;
ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
ground_speed = parsenumber(parseptr, 1) * 100 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true;
} else {
_error("GPSERR: Checksum error!!\n");
return false;
}
}
} else {
bufferidx = 0;
_error("GPSERR: Bad sentence!!\n");
return false;
}
return true;
}
// Parse hexadecimal numbers
uint8_t
AP_GPS_NMEA::parseHex(char c) {
if (c < '0')
return (0);
if (c <= '9')
return (c - '0');
if (c < 'A')
return (0);
if (c <= 'F')
return ((c - 'A')+10);
}
// Decimal number parser
long
AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) {
long d = 0;
uint8_t i;
i = num_car;
while ((str[0] != 0) && (i > 0)) {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
str++;
i--;
}
return d;
}
// Function to parse fixed point numbers (numdec=number of decimals)
long
AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) {
long d = 0;
uint8_t ndec = 0;
while (str[0] != 0) {
if (str[0] == '.'){
ndec = 1;
} else {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
if (ndec > 0)
ndec++;
if (ndec > numdec) // we reach the number of decimals...
return d;
}
str++;
}
return d;
}