Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: support a wider variety of NMEA receivers

Browse Source 
some reeivers use a different talker ID. This allows us to accept
them.
This commit is contained in:
Andrew Tridgell 2016-04-18 15:30:17 +10:00
parent 0d9b9433da
commit 2a214f1e46
2 changed files with 62 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -91,12 +91,6 @@ extern const AP_HAL::HAL& hal;
const char AP_GPS_NMEA::_initialisation_blob[] = SIRF_INIT_MSG MTK_INIT_MSG UBLOX_INIT_MSG; const char AP_GPS_NMEA::_initialisation_blob[] = SIRF_INIT_MSG MTK_INIT_MSG UBLOX_INIT_MSG;
// NMEA message identifiers ////////////////////////////////////////////////////
//
const char AP_GPS_NMEA::_gprmc_string[] = "GPRMC";
const char AP_GPS_NMEA::_gpgga_string[] = "GPGGA";
const char AP_GPS_NMEA::_gpvtg_string[] = "GPVTG";
// Convenience macros ////////////////////////////////////////////////////////// // Convenience macros //////////////////////////////////////////////////////////
// //
#define DIGIT_TO_VAL(_x) (_x - '0') #define DIGIT_TO_VAL(_x) (_x - '0')
@ -257,25 +251,25 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
*/ */
bool AP_GPS_NMEA::_have_new_message() bool AP_GPS_NMEA::_have_new_message()
{ {
if (_last_GPRMC_ms == 0 || if (_last_RMC_ms == 0 ||
_last_GPGGA_ms == 0) { _last_GGA_ms == 0) {
return false; return false;
} }
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - _last_GPRMC_ms > 150 || if (now - _last_RMC_ms > 150 ||
now - _last_GPGGA_ms > 150) { now - _last_GGA_ms > 150) {
return false; return false;
} }
if (_last_GPVTG_ms != 0 && if (_last_VTG_ms != 0 &&
now - _last_GPVTG_ms > 150) { now - _last_VTG_ms > 150) {
return false; return false;
} }
// prevent these messages being used again // prevent these messages being used again
if (_last_GPVTG_ms != 0) { if (_last_VTG_ms != 0) {
_last_GPVTG_ms = 1; _last_VTG_ms = 1;
} }
_last_GPGGA_ms = 1; _last_GGA_ms = 1;
_last_GPRMC_ms = 1; _last_RMC_ms = 1;
return true; return true;
} }
@ -290,8 +284,8 @@ bool AP_GPS_NMEA::_term_complete()
if (_gps_data_good) { if (_gps_data_good) {
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
switch (_sentence_type) { switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC: case _GPS_SENTENCE_RMC:
_last_GPRMC_ms = now; _last_RMC_ms = now;
//time = _new_time; //time = _new_time;
//date = _new_date; //date = _new_date;
state.location.lat = _new_latitude; state.location.lat = _new_latitude;
@ -304,8 +298,8 @@ bool AP_GPS_NMEA::_term_complete()
state.status = AP_GPS::GPS_OK_FIX_3D; state.status = AP_GPS::GPS_OK_FIX_3D;
fill_3d_velocity(); fill_3d_velocity();
break; break;
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GGA:
_last_GPGGA_ms = now; _last_GGA_ms = now;
state.location.alt = _new_altitude; state.location.alt = _new_altitude;
state.location.lat = _new_latitude; state.location.lat = _new_latitude;
state.location.lng = _new_longitude; state.location.lng = _new_longitude;
@ -314,8 +308,8 @@ bool AP_GPS_NMEA::_term_complete()
// To-Do: add support for proper reporting of 2D and 3D fix // To-Do: add support for proper reporting of 2D and 3D fix
state.status = AP_GPS::GPS_OK_FIX_3D; state.status = AP_GPS::GPS_OK_FIX_3D;
break; break;
case _GPS_SENTENCE_GPVTG: case _GPS_SENTENCE_VTG:
_last_GPVTG_ms = now; _last_VTG_ms = now;
state.ground_speed = _new_speed*0.01f; state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = wrap_360_cd(_new_course); state.ground_course_cd = wrap_360_cd(_new_course);
fill_3d_velocity(); fill_3d_velocity();
@ -324,8 +318,8 @@ bool AP_GPS_NMEA::_term_complete()
} }
} else { } else {
switch (_sentence_type) { switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC: case _GPS_SENTENCE_RMC:
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GGA:
// Only these sentences give us information about // Only these sentences give us information about
// fix status. // fix status.
state.status = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX;
@ -340,12 +334,23 @@ bool AP_GPS_NMEA::_term_complete()
// the first term determines the sentence type // the first term determines the sentence type
if (_term_number == 0) { if (_term_number == 0) {
if (!strcmp(_term, _gprmc_string)) { /*
_sentence_type = _GPS_SENTENCE_GPRMC; The first two letters of the NMEA term are the talker
} else if (!strcmp(_term, _gpgga_string)) { ID. The most common is 'GP' but there are a bunch of others
_sentence_type = _GPS_SENTENCE_GPGGA; that are valid. We accept any two characters here.
} else if (!strcmp(_term, _gpvtg_string)) { */
_sentence_type = _GPS_SENTENCE_GPVTG; if (_term[0] < 'A' || _term[0] > 'Z' ||
_term[1] < 'A' || _term[1] > 'Z') {
_sentence_type = _GPS_SENTENCE_OTHER;
return false;
}
const char *term_type = &_term[2];
if (strcmp(term_type, "RMC") == 0) {
_sentence_type = _GPS_SENTENCE_RMC;
} else if (strcmp(term_type, "GGA") == 0) {
_sentence_type = _GPS_SENTENCE_GGA;
} else if (strcmp(term_type, "VTG") == 0) {
_sentence_type = _GPS_SENTENCE_VTG;
// VTG may not contain a data qualifier, presume the solution is good // VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise. // unless it tells us otherwise.
_gps_data_good = true; _gps_data_good = true;
@ -360,64 +365,64 @@ bool AP_GPS_NMEA::_term_complete()
switch (_sentence_type + _term_number) { switch (_sentence_type + _term_number) {
// operational status // operational status
// //
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC) case _GPS_SENTENCE_RMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A'; _gps_data_good = _term[0] == 'A';
break; break;
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA) case _GPS_SENTENCE_GGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0'; _gps_data_good = _term[0] > '0';
break; break;
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field) case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N'; _gps_data_good = _term[0] != 'N';
break; break;
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA) case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term); _new_satellite_count = atol(_term);
break; break;
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA) case _GPS_SENTENCE_GGA + 8: // HDOP (GGA)
_new_hdop = _parse_decimal_100(); _new_hdop = _parse_decimal_100();
break; break;
// time and date // time and date
// //
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC) case _GPS_SENTENCE_RMC + 1: // Time (RMC)
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA) case _GPS_SENTENCE_GGA + 1: // Time (GGA)
_new_time = _parse_decimal_100(); _new_time = _parse_decimal_100();
break; break;
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC) case _GPS_SENTENCE_RMC + 9: // Date (GPRMC)
_new_date = atol(_term); _new_date = atol(_term);
break; break;
// location // location
// //
case _GPS_SENTENCE_GPRMC + 3: // Latitude case _GPS_SENTENCE_RMC + 3: // Latitude
case _GPS_SENTENCE_GPGGA + 2: case _GPS_SENTENCE_GGA + 2:
_new_latitude = _parse_degrees(); _new_latitude = _parse_degrees();
break; break;
case _GPS_SENTENCE_GPRMC + 4: // N/S case _GPS_SENTENCE_RMC + 4: // N/S
case _GPS_SENTENCE_GPGGA + 3: case _GPS_SENTENCE_GGA + 3:
if (_term[0] == 'S') if (_term[0] == 'S')
_new_latitude = -_new_latitude; _new_latitude = -_new_latitude;
break; break;
case _GPS_SENTENCE_GPRMC + 5: // Longitude case _GPS_SENTENCE_RMC + 5: // Longitude
case _GPS_SENTENCE_GPGGA + 4: case _GPS_SENTENCE_GGA + 4:
_new_longitude = _parse_degrees(); _new_longitude = _parse_degrees();
break; break;
case _GPS_SENTENCE_GPRMC + 6: // E/W case _GPS_SENTENCE_RMC + 6: // E/W
case _GPS_SENTENCE_GPGGA + 5: case _GPS_SENTENCE_GGA + 5:
if (_term[0] == 'W') if (_term[0] == 'W')
_new_longitude = -_new_longitude; _new_longitude = -_new_longitude;
break; break;
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA) case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal_100(); _new_altitude = _parse_decimal_100();
break; break;
// course and speed // course and speed
// //
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC) case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG) case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal_100() * 514) / 1000; // knots-> m/sec, approximiates * 0.514 _new_speed = (_parse_decimal_100() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break; break;
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC) case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG) case _GPS_SENTENCE_VTG + 1: // Course (VTG)
_new_course = _parse_decimal_100(); _new_course = _parse_decimal_100();
break; break;
} }

19
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -64,9 +64,9 @@ public:
private: private:
/// Coding for the GPS sentences that the parser handles /// Coding for the GPS sentences that the parser handles
enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value. enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value.
_GPS_SENTENCE_GPRMC = 32, _GPS_SENTENCE_RMC = 32,
_GPS_SENTENCE_GPGGA = 64, _GPS_SENTENCE_GGA = 64,
_GPS_SENTENCE_GPVTG = 96, _GPS_SENTENCE_VTG = 96,
_GPS_SENTENCE_OTHER = 0 _GPS_SENTENCE_OTHER = 0
}; };
@ -137,9 +137,9 @@ private:
uint16_t _new_hdop; ///< HDOP parsed from a term uint16_t _new_hdop; ///< HDOP parsed from a term
uint8_t _new_satellite_count; ///< satellite count parsed from a term uint8_t _new_satellite_count; ///< satellite count parsed from a term
uint32_t _last_GPRMC_ms = 0; uint32_t _last_RMC_ms = 0;
uint32_t _last_GPGGA_ms = 0; uint32_t _last_GGA_ms = 0;
uint32_t _last_GPVTG_ms = 0; uint32_t _last_VTG_ms = 0;
/// @name Init strings /// @name Init strings
/// In ::init, an attempt is made to configure the GPS /// In ::init, an attempt is made to configure the GPS
@ -151,12 +151,5 @@ private:
static const char _ublox_init_string[]; ///< init string for ublox units static const char _ublox_init_string[]; ///< init string for ublox units
//@} //@}
/// @name GPS message identifier strings
//@{
static const char _gprmc_string[];
static const char _gpgga_string[];
static const char _gpvtg_string[];
//@}
static const char _initialisation_blob[]; static const char _initialisation_blob[];
}; };