rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

Flight/GPS: Some updates

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2011 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
sambas 2010-10-27 10:39:38 +00:00 committed by sambas
parent a568e27703
commit f1ba558fa1
1 changed files with 133 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
flight/OpenPilot/Modules/GPS/NMEA.c
View File

@ -15,6 +15,7 @@
#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
#define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages
#define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages
#define NMEA_DEBUG_PGTOP ///< define to enable debug of PGTOP messages
#endif
/* Utility functions */
@ -34,6 +35,7 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessPGTOP(GPSPositionData * GpsData, char *sentence);
static struct nmea_parser nmea_parsers[] = {
{
@ -60,6 +62,10 @@ static struct nmea_parser nmea_parsers[] = {
.prefix = "GPGSV",
.handler = nmeaProcessGPGSV,
},
{
.prefix = "PGTOP",
.handler = nmeaProcessPGTOP,
},
};
static struct nmea_parser *NMEA_find_parser_by_prefix(char *prefix)
@ -183,7 +189,7 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence)
return false;
}
// next field: N/S indicator
// correct latitute for N/S
// correct latitude for N/S
tokens = strsep(&next, delimiter);
if (tokens[0] == 'S')
GpsData->Latitude = -GpsData->Latitude;
@ -277,7 +283,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence)
return false;
}
// next field: N/S indicator
// correct latitute for N/S
// correct latitude for N/S
tokens = strsep(&next, delimiter);
if (tokens[0] == 'S')
GpsData->Latitude = -GpsData->Latitude;
@ -289,7 +295,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence)
return false;
}
// next field: E/W indicator
// correct latitute for E/W
// correct longitude for E/W
tokens = strsep(&next, delimiter);
if (tokens[0] == 'W')
GpsData->Longitude = -GpsData->Longitude;
@ -298,6 +304,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence)
// get speed in knots
tokens = strsep(&next, delimiter);
GpsData->Groundspeed = NMEA_real_to_float(tokens);
// to m/s
GpsData->Groundspeed *= 0.51444;
// next field: True course
@ -348,6 +355,7 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence)
// get course (true north ref) in degrees [ddd.dd]
tokens = strsep(&next, delimiter);
GpsData->Heading = NMEA_real_to_float(tokens);
// next field: 'T'
tokens = strsep(&next, delimiter);
@ -361,6 +369,10 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence)
// next field: speed (knots)
// get speed in knots
tokens = strsep(&next, delimiter);
GpsData->Groundspeed = NMEA_real_to_float(tokens);
// to m/s
GpsData->Groundspeed *= 0.51444;
// next field: 'N'
tokens = strsep(&next, delimiter);
@ -583,6 +595,123 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence)
return true;
}
/**
* Parse an NMEA PGTOP sentence and update the given UAVObject
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessPGTOP(GPSPositionData * GpsData, char *sentence)
{
char *next = sentence;
char *tokens;
char *delimiter = ",*";
#ifdef NMEA_DEBUG_PGTOP
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
#endif
GPSTimeData gpst;
GPSTimeGet(&gpst);
// get UTC time [hhmmss.sss]
tokens = strsep(&next, delimiter);
float hms = NMEA_real_to_float(tokens);
gpst.Second = (int)hms % 100;
gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
gpst.Hour = (int)hms / 10000;
// next field: latitude
// get latitude [ddmm.mmmmm]
tokens = strsep(&next, delimiter);
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, tokens)) {
return false;
}
// next field: N/S indicator
// correct latitude for N/S
tokens = strsep(&next, delimiter);
if (tokens[0] == 'S')
GpsData->Latitude = -GpsData->Latitude;
// next field: longitude
// get longitude [dddmm.mmmmm]
tokens = strsep(&next, delimiter);
if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, tokens)) {
return false;
}
// next field: E/W indicator
// correct longitude for E/W
tokens = strsep(&next, delimiter);
if (tokens[0] == 'W')
GpsData->Longitude = -GpsData->Longitude;
// next field: Fix Quality
// Mode: 0=Fix not available, 1=GPS fix, 2=DGPS fix
tokens = strsep(&next, delimiter);
// next field: satellites used
// get number of satellites used in GPS solution
tokens = strsep(&next, delimiter);
GpsData->Satellites = atoi(tokens);
// next field: HDOP
tokens = strsep(&next, delimiter);
GpsData->HDOP = NMEA_real_to_float(tokens);
// next field: altitude
// get altitude (in meters mm.m)
tokens = strsep(&next, delimiter);
GpsData->Altitude = NMEA_real_to_float(tokens);
// next field: geoid separation
tokens = strsep(&next, delimiter);
GpsData->GeoidSeparation = NMEA_real_to_float(tokens);
// next field: Fix Type
// Mode: 1=Fix not available, 2=2D, 3=3D
tokens = strsep(&next, delimiter);
switch (atoi(tokens)) {
case 1:
GpsData->Status = GPSPOSITION_STATUS_NOFIX;
break;
case 2:
GpsData->Status = GPSPOSITION_STATUS_FIX2D;
break;
case 3:
GpsData->Status = GPSPOSITION_STATUS_FIX3D;
break;
default:
/* Unhandled */
return false;
break;
}
// get course over ground in degrees [ddd.dd]
tokens = strsep(&next, delimiter);
GpsData->Heading = NMEA_real_to_float(tokens);
// next field: speed (km/h)
// get speed in km/h
tokens = strsep(&next, delimiter);
GpsData->Groundspeed = NMEA_real_to_float(tokens);
// to m/s
GpsData->Groundspeed /= 3.6;
tokens = strsep(&next, delimiter);
gpst.Day = (uint8_t) NMEA_real_to_float(next);
tokens = strsep(&next, delimiter);
gpst.Month = (uint8_t) NMEA_real_to_float(next);
tokens = strsep(&next, delimiter);
gpst.Year = (uint16_t) NMEA_real_to_float(next);
GPSTimeSet(&gpst);
// next field: checksum
tokens = strsep(&next, delimiter);
return true;
}
/* Parse a number encoded in a string of the format:
* [-]NN.nnnnn
* into a signed whole part and an unsigned fractional part.