mirror of https://github.com/ArduPilot/ardupilot
420 lines
13 KiB
C++
420 lines
13 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
//
|
|
// NMEA parser, adapted by Michael Smith from TinyGPS v9:
|
|
//
|
|
// TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
|
|
// Copyright (C) 2008-9 Mikal Hart
|
|
// All rights reserved.
|
|
//
|
|
|
|
/// @file AP_GPS_NMEA.cpp
|
|
/// @brief NMEA protocol parser
|
|
///
|
|
/// This is a lightweight NMEA parser, derived originally from the
|
|
/// TinyGPS parser by Mikal Hart.
|
|
///
|
|
|
|
#include <AP_Common.h>
|
|
|
|
#include <AP_Progmem.h>
|
|
#include <ctype.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "AP_GPS_NMEA.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
// SiRF init messages //////////////////////////////////////////////////////////
|
|
//
|
|
// Note that we will only see a SiRF in NMEA mode if we are explicitly configured
|
|
// for NMEA. GPS_AUTO will try to set any SiRF unit to binary mode as part of
|
|
// the autodetection process.
|
|
//
|
|
const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
|
|
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
|
|
"$PSRF103,1,0,0,1*25\r\n" // GLL off
|
|
"$PSRF103,2,0,0,1*26\r\n" // GSA off
|
|
"$PSRF103,3,0,0,1*27\r\n" // GSV off
|
|
"$PSRF103,4,0,1,1*20\r\n" // RMC off
|
|
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
|
|
"$PSRF103,6,0,0,1*22\r\n" // MSS off
|
|
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off
|
|
"$PSRF151,1*3F\r\n" // WAAS on (not always supported)
|
|
"$PSRF106,21*0F\r\n" // datum = WGS84
|
|
"";
|
|
|
|
// MediaTek init messages //////////////////////////////////////////////////////
|
|
//
|
|
// Note that we may see a MediaTek in NMEA mode if we are connected to a non-DIYDrones
|
|
// MediaTek-based GPS.
|
|
//
|
|
const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
|
|
"$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix
|
|
"$PMTK330,0*2E\r\n" // datum = WGS84
|
|
"$PMTK313,1*2E\r\n" // SBAS on
|
|
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS
|
|
"";
|
|
|
|
// ublox init messages /////////////////////////////////////////////////////////
|
|
//
|
|
// Note that we will only see a ublox in NMEA mode if we are explicitly configured
|
|
// for NMEA. GPS_AUTO will try to set any ublox unit to binary mode as part of
|
|
// the autodetection process.
|
|
//
|
|
// We don't attempt to send $PUBX,41 as the unit must already be talking NMEA
|
|
// and we don't know the baudrate.
|
|
//
|
|
const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM =
|
|
"$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix
|
|
"$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix
|
|
"$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?)
|
|
"";
|
|
|
|
// NMEA message identifiers ////////////////////////////////////////////////////
|
|
//
|
|
const char AP_GPS_NMEA::_gprmc_string[] PROGMEM = "GPRMC";
|
|
const char AP_GPS_NMEA::_gpgga_string[] PROGMEM = "GPGGA";
|
|
const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
|
|
|
|
// Convenience macros //////////////////////////////////////////////////////////
|
|
//
|
|
#define DIGIT_TO_VAL(_x) (_x - '0')
|
|
|
|
// Public Methods //////////////////////////////////////////////////////////////
|
|
void AP_GPS_NMEA::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
|
|
{
|
|
_port = s;
|
|
|
|
// send the SiRF init strings
|
|
_port->print_P((const prog_char_t *)_SiRF_init_string);
|
|
|
|
// send the MediaTek init strings
|
|
_port->print_P((const prog_char_t *)_MTK_init_string);
|
|
|
|
// send the ublox init strings
|
|
_port->print_P((const prog_char_t *)_ublox_init_string);
|
|
}
|
|
|
|
bool AP_GPS_NMEA::read(void)
|
|
{
|
|
int16_t numc;
|
|
bool parsed = false;
|
|
|
|
numc = _port->available();
|
|
while (numc--) {
|
|
if (_decode(_port->read())) {
|
|
parsed = true;
|
|
}
|
|
}
|
|
return parsed;
|
|
}
|
|
|
|
bool AP_GPS_NMEA::_decode(char c)
|
|
{
|
|
bool valid_sentence = false;
|
|
|
|
switch (c) {
|
|
case ',': // term terminators
|
|
_parity ^= c;
|
|
case '\r':
|
|
case '\n':
|
|
case '*':
|
|
if (_term_offset < sizeof(_term)) {
|
|
_term[_term_offset] = 0;
|
|
valid_sentence = _term_complete();
|
|
}
|
|
++_term_number;
|
|
_term_offset = 0;
|
|
_is_checksum_term = c == '*';
|
|
return valid_sentence;
|
|
|
|
case '$': // sentence begin
|
|
_term_number = _term_offset = 0;
|
|
_parity = 0;
|
|
_sentence_type = _GPS_SENTENCE_OTHER;
|
|
_is_checksum_term = false;
|
|
_gps_data_good = false;
|
|
return valid_sentence;
|
|
}
|
|
|
|
// ordinary characters
|
|
if (_term_offset < sizeof(_term) - 1)
|
|
_term[_term_offset++] = c;
|
|
if (!_is_checksum_term)
|
|
_parity ^= c;
|
|
|
|
return valid_sentence;
|
|
}
|
|
|
|
//
|
|
// internal utilities
|
|
//
|
|
int16_t AP_GPS_NMEA::_from_hex(char a)
|
|
{
|
|
if (a >= 'A' && a <= 'F')
|
|
return a - 'A' + 10;
|
|
else if (a >= 'a' && a <= 'f')
|
|
return a - 'a' + 10;
|
|
else
|
|
return a - '0';
|
|
}
|
|
|
|
uint32_t AP_GPS_NMEA::_parse_decimal_100()
|
|
{
|
|
char *p = _term;
|
|
uint32_t ret = 100UL * atol(p);
|
|
while (isdigit(*p))
|
|
++p;
|
|
if (*p == '.') {
|
|
if (isdigit(p[1])) {
|
|
ret += 10 * (p[1] - '0');
|
|
if (isdigit(p[2]))
|
|
ret += p[2] - '0';
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
parse a NMEA latitude/longitude degree value. The result is in degrees*1e7
|
|
*/
|
|
uint32_t AP_GPS_NMEA::_parse_degrees()
|
|
{
|
|
char *p, *q;
|
|
uint8_t deg = 0, min = 0;
|
|
float frac_min = 0;
|
|
int32_t ret = 0;
|
|
|
|
// scan for decimal point or end of field
|
|
for (p = _term; isdigit(*p); p++)
|
|
;
|
|
q = _term;
|
|
|
|
// convert degrees
|
|
while ((p - q) > 2) {
|
|
if (deg)
|
|
deg *= 10;
|
|
deg += DIGIT_TO_VAL(*q++);
|
|
}
|
|
|
|
// convert minutes
|
|
while (p > q) {
|
|
if (min)
|
|
min *= 10;
|
|
min += DIGIT_TO_VAL(*q++);
|
|
}
|
|
|
|
// convert fractional minutes
|
|
if (*p == '.') {
|
|
q = p + 1;
|
|
float frac_scale = 0.1f;
|
|
while (isdigit(*q)) {
|
|
frac_min += (*q++ - '0') * frac_scale;
|
|
frac_scale *= 0.1f;
|
|
}
|
|
}
|
|
ret = (deg * (int32_t)10000000UL);
|
|
ret += (min * (int32_t)10000000UL / 60);
|
|
ret += (int32_t) (frac_min * (1.0e7 / 60.0f));
|
|
return ret;
|
|
}
|
|
|
|
// Processes a just-completed term
|
|
// Returns true if new sentence has just passed checksum test and is validated
|
|
bool AP_GPS_NMEA::_term_complete()
|
|
{
|
|
// handle the last term in a message
|
|
if (_is_checksum_term) {
|
|
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
|
|
if (checksum == _parity) {
|
|
if (_gps_data_good) {
|
|
switch (_sentence_type) {
|
|
case _GPS_SENTENCE_GPRMC:
|
|
//time = _new_time;
|
|
//date = _new_date;
|
|
latitude = _new_latitude;
|
|
longitude = _new_longitude;
|
|
ground_speed_cm = _new_speed;
|
|
ground_course_cd = _new_course;
|
|
_make_gps_time(_new_date, _new_time * 10);
|
|
_last_gps_time = hal.scheduler->millis();
|
|
fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA:
|
|
altitude_cm = _new_altitude;
|
|
//time = _new_time;
|
|
latitude = _new_latitude;
|
|
longitude = _new_longitude;
|
|
num_sats = _new_satellite_count;
|
|
hdop = _new_hdop;
|
|
fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix
|
|
break;
|
|
case _GPS_SENTENCE_GPVTG:
|
|
ground_speed_cm = _new_speed;
|
|
ground_course_cd = _new_course;
|
|
// VTG has no fix indicator, can't change fix status
|
|
break;
|
|
}
|
|
} else {
|
|
switch (_sentence_type) {
|
|
case _GPS_SENTENCE_GPRMC:
|
|
case _GPS_SENTENCE_GPGGA:
|
|
// Only these sentences give us information about
|
|
// fix status.
|
|
fix = GPS::FIX_NONE;
|
|
}
|
|
}
|
|
// we got a good message
|
|
return true;
|
|
}
|
|
// we got a bad message, ignore it
|
|
return false;
|
|
}
|
|
|
|
// the first term determines the sentence type
|
|
if (_term_number == 0) {
|
|
if (!strcmp_P(_term, _gprmc_string)) {
|
|
_sentence_type = _GPS_SENTENCE_GPRMC;
|
|
} else if (!strcmp_P(_term, _gpgga_string)) {
|
|
_sentence_type = _GPS_SENTENCE_GPGGA;
|
|
} else if (!strcmp_P(_term, _gpvtg_string)) {
|
|
_sentence_type = _GPS_SENTENCE_GPVTG;
|
|
// VTG may not contain a data qualifier, presume the solution is good
|
|
// unless it tells us otherwise.
|
|
_gps_data_good = true;
|
|
} else {
|
|
_sentence_type = _GPS_SENTENCE_OTHER;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// 32 = RMC, 64 = GGA, 96 = VTG
|
|
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
|
|
switch (_sentence_type + _term_number) {
|
|
// operational status
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC)
|
|
_gps_data_good = _term[0] == 'A';
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA)
|
|
_gps_data_good = _term[0] > '0';
|
|
break;
|
|
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field)
|
|
_gps_data_good = _term[0] != 'N';
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA)
|
|
_new_satellite_count = atol(_term);
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA)
|
|
_new_hdop = _parse_decimal_100();
|
|
break;
|
|
|
|
// time and date
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC)
|
|
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA)
|
|
_new_time = _parse_decimal_100();
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC)
|
|
_new_date = atol(_term);
|
|
break;
|
|
|
|
// location
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 3: // Latitude
|
|
case _GPS_SENTENCE_GPGGA + 2:
|
|
_new_latitude = _parse_degrees();
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 4: // N/S
|
|
case _GPS_SENTENCE_GPGGA + 3:
|
|
if (_term[0] == 'S')
|
|
_new_latitude = -_new_latitude;
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 5: // Longitude
|
|
case _GPS_SENTENCE_GPGGA + 4:
|
|
_new_longitude = _parse_degrees();
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 6: // E/W
|
|
case _GPS_SENTENCE_GPGGA + 5:
|
|
if (_term[0] == 'W')
|
|
_new_longitude = -_new_longitude;
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA)
|
|
_new_altitude = _parse_decimal_100();
|
|
break;
|
|
|
|
// course and speed
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC)
|
|
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG)
|
|
_new_speed = (_parse_decimal_100() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC)
|
|
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG)
|
|
_new_course = _parse_decimal_100();
|
|
break;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
#define hexdigit(x) ((x)>9?'A'+(x):'0'+(x))
|
|
|
|
/*
|
|
detect a NMEA GPS. Adds one byte, and returns true if the stream
|
|
matches a NMEA string
|
|
*/
|
|
bool
|
|
AP_GPS_NMEA::_detect(uint8_t data)
|
|
{
|
|
static uint8_t step;
|
|
static uint8_t ck;
|
|
|
|
switch (step) {
|
|
case 0:
|
|
ck = 0;
|
|
if ('$' == data) {
|
|
step++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if ('*' == data) {
|
|
step++;
|
|
} else {
|
|
ck ^= data;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (hexdigit(ck>>4) == data) {
|
|
step++;
|
|
} else {
|
|
step = 0;
|
|
}
|
|
break;
|
|
case 3:
|
|
if (hexdigit(ck&0xF) == data) {
|
|
return true;
|
|
}
|
|
step = 0;
|
|
break;
|
|
}
|
|
return false;
|
|
}
|