rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00
Code Issues Releases Activity

Allow multiples talkers IDs, define some debug messages and typos

Browse Source
This commit is contained in:
Laurent Lalanne 2015-02-06 19:50:09 +01:00
parent 58e70b2e3a
commit 5c08f013f1
1 changed files with 39 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
flight/modules/GPS/NMEA.c
View File

@ -40,14 +40,13 @@
#include "GPS.h" #include "GPS.h"
// #define ENABLE_DEBUG_MSG ///< define to enable debug-messages // #define ENABLE_DEBUG_MSG ///< define to enable debug-messages
#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages #define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is used for debug-messages
// Debugging // Debugging
#ifdef ENABLE_DEBUG_MSG #ifdef ENABLE_DEBUG_MSG
// #define DEBUG_MSG_IN ///< define to display the incoming NMEA messages // #define DEBUG_MSG_IN ///< define to display the incoming NMEA messages
// #define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters // #define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters
// #define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages // #define DEBUG_MSGID_IN ///< define to display the names of the incoming NMEA messages
// #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages // #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
// #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages // #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
// #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages // #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages
@ -68,40 +67,40 @@ struct nmea_parser {
bool (*handler)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); bool (*handler)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
}; };
static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
static bool nmeaProcessGPZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); static bool nmeaProcessGxZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); static bool nmeaProcessGxGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
#endif // PIOS_GPS_MINIMAL #endif // PIOS_GPS_MINIMAL
static const struct nmea_parser nmea_parsers[] = { static const struct nmea_parser nmea_parsers[] = {
{ {
.prefix = "GPGGA", .prefix = "GGA",
.handler = nmeaProcessGPGGA, .handler = nmeaProcessGxGGA,
}, },
{ {
.prefix = "GPVTG", .prefix = "VTG",
.handler = nmeaProcessGPVTG, .handler = nmeaProcessGxVTG,
}, },
{ {
.prefix = "GPGSA", .prefix = "GSA",
.handler = nmeaProcessGPGSA, .handler = nmeaProcessGxGSA,
}, },
{ {
.prefix = "GPRMC", .prefix = "RMC",
.handler = nmeaProcessGPRMC, .handler = nmeaProcessGxRMC,
}, },
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
{ {
.prefix = "GPZDA", .prefix = "ZDA",
.handler = nmeaProcessGPZDA, .handler = nmeaProcessGxZDA,
}, },
{ {
.prefix = "GPGSV", .prefix = "GSV",
.handler = nmeaProcessGPGSV, .handler = nmeaProcessGxGSV,
}, },
#endif // PIOS_GPS_MINIMAL #endif // PIOS_GPS_MINIMAL
}; };
@ -373,6 +372,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
// Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43" // Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43"
// The first parameter starts at the beginning of the message // The first parameter starts at the beginning of the message
// Skip first two character, allow GL, GN, GP...
p += 2;
params[0] = p; params[0] = p;
nbParams = 1; nbParams = 1;
while (*p != 0) { while (*p != 0) {
@ -406,11 +407,13 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
parser = NMEA_find_parser_by_prefix(params[0]); parser = NMEA_find_parser_by_prefix(params[0]);
if (!parser) { if (!parser) {
// No parser found // No parser found
#ifdef DEBUG_MSGID_IN
DEBUG_MSG(" NO PARSER (\"%s\")\n", params[0]); DEBUG_MSG(" NO PARSER (\"%s\")\n", params[0]);
#endif
return false; return false;
} }
#ifdef DEBUG_MGSID_IN #ifdef DEBUG_MSGID_IN
DEBUG_MSG("%s %d ", params[0]); DEBUG_MSG("%s %d ", params[0]);
#endif #endif
// Send the message to the parser and get it update the GpsData // Send the message to the parser and get it update the GpsData
@ -422,7 +425,9 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
// Parse failed // Parse failed
#ifdef DEBUG_MSGID_IN
DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
#endif
if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) { if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
GPSPositionSensorSet(GpsData); GPSPositionSensorSet(GpsData);
} }
@ -432,13 +437,13 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
// All is fine :) Update object if data has changed // All is fine :) Update object if data has changed
if (gpsDataUpdated) { if (gpsDataUpdated) {
#ifdef DEBUG_MGSID_IN #ifdef DEBUG_MSGID_IN
DEBUG_MSG("U"); DEBUG_MSG("U");
#endif #endif
GPSPositionSensorSet(GpsData); GPSPositionSensorSet(GpsData);
} }
#ifdef DEBUG_MGSID_IN #ifdef DEBUG_MSGID_IN
DEBUG_MSG("\n"); DEBUG_MSG("\n");
#endif #endif
return true; return true;
@ -446,11 +451,11 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
/** /**
* Parse an NMEA GPGGA sentence and update the given UAVObject * Parse an NMEA GxGGA sentence and update the given UAVObject
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum * \param[in] An NMEA sentence with a valid checksum
*/ */
static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{ {
if (nbParam != 15) { if (nbParam != 15) {
return false; return false;
@ -499,11 +504,11 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
} }
/** /**
* Parse an NMEA GPRMC sentence and update the given UAVObject * Parse an NMEA GxRMC sentence and update the given UAVObject
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum * \param[in] An NMEA sentence with a valid checksum
*/ */
static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{ {
if (nbParam != 13) { if (nbParam != 13) {
return false; return false;
@ -567,11 +572,11 @@ static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
} }
/** /**
* Parse an NMEA GPVTG sentence and update the given UAVObject * Parse an NMEA GxVTG sentence and update the given UAVObject
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum * \param[in] An NMEA sentence with a valid checksum
*/ */
static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{ {
if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) { if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) {
return false; return false;
@ -592,11 +597,11 @@ static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
/** /**
* Parse an NMEA GPZDA sentence and update the @ref GPSTime object * Parse an NMEA GxZDA sentence and update the @ref GPSTime object
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused). * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused).
* \param[in] An NMEA sentence with a valid checksum * \param[in] An NMEA sentence with a valid checksum
*/ */
static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) static bool nmeaProcessGxZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{ {
if (nbParam != 7) { if (nbParam != 7) {
return false; return false;
@ -636,7 +641,7 @@ static uint8_t gsv_processed_mask;
static uint16_t gsv_incomplete_error; static uint16_t gsv_incomplete_error;
static uint16_t gsv_duplicate_error; static uint16_t gsv_duplicate_error;
static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) static bool nmeaProcessGxGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{ {
if (nbParam < 4) { if (nbParam < 4) {
return false; return false;
@ -723,7 +728,7 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsD
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum * \param[in] An NMEA sentence with a valid checksum
*/ */
static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{ {
if (nbParam != 18) { if (nbParam != 18) {
return false; return false;