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

gps: Update GPSPosition instead of PositionActual

Browse Source 
GPS module now updates GPSPosition UAVObject
rather than the PositionActual object.  The
GPSPosition object is intended to be consumed
only by the AHRS.  The AHRS will use this (and
other inputs) to compute a filtered version of
the position in the PositionActual object.

This commit will cause temporary breakage of the
GPS functionality in the GCS until the PositionActual
object is properly updated by the AHRS.  Most of the
GCS should continue to use PositionActual.  The only
exception to this might be any tool for specifically
visualizing the raw GPS state.

GPS.c is now only responsible for receiving a
complete NMEA sentence from the COM interface.

NMEA parsing is now factored out into NMEA.[ch]
which is where GPSPosition is now updated based
on the complete NMEA sentences obtained from the
GPS.

Latitude and Longitude are now encoded in a
fixed-point notation in units of degrees x 10^-7
to prevent truncation of precision due to encoding
into a float.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1431 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
stac 2010-08-27 02:15:00 +00:00 committed by stac
parent 47df1faa8d
commit 24a4c6b791
4 changed files with 702 additions and 658 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -58,7 +58,7 @@
#include "alarms.h"
#include "baroaltitude.h"
#include "stdbool.h"
#include "positionactual.h"
#include "gpsposition.h"
#include "homelocation.h"
#include "ahrscalibration.h"
#include "CoordinateConversions.h"
@ -128,7 +128,7 @@ int32_t AHRSCommsInitialize(void)
{
AHRSSettingsConnectCallback(AHRSSettingsUpdatedCb);
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
PositionActualConnectCallback(GPSPositionUpdatedCb);
GPSPositionConnectCallback(GPSPositionUpdatedCb);
HomeLocationConnectCallback(HomeLocationUpdatedCb);
AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb);
@ -393,8 +393,8 @@ static void load_baro_altitude(struct opahrs_msg_v1_req_update * update)
static void load_gps_position(struct opahrs_msg_v1_req_update * update)
{
PositionActualData data;
PositionActualGet(&data);
GPSPositionData data;
GPSPositionGet(&data);
HomeLocationData home;
HomeLocationGet(&home);
@ -411,8 +411,7 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update)
update->gps.groundspeed = data.Groundspeed;
update->gps.heading = data.Heading;
update->gps.quality = 0;
// TODO: replace with conversion from degrees * 10e6 to degrees when the GPS format updated
double LLA[3] = {(double) data.Latitude, (double) data.Longitude, (double) data.Altitude};
double LLA[3] = {(double) data.Latitude / 1e7, (double) data.Longitude / 1e7, (double) data.Altitude};
// convert from cm back to meters
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, update->gps.NED);
@ -462,7 +461,7 @@ static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituder
data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = attituderaw->gyros_filtered.z;
data.gyrotemp[ATTITUDERAW_GYROTEMP_XY] = attituderaw->gyros.xy_temp;
data.gyrotemp[ATTITUDERAW_GYROTEMP_Z] = attituderaw->gyros.z_temp;
data.gyrotemp[ATTITUDERAW_GYROTEMP_Z] = attituderaw->gyros.z_temp;
data.accels[ATTITUDERAW_ACCELS_X] = attituderaw->accels.x;
data.accels[ATTITUDERAW_ACCELS_Y] = attituderaw->accels.y;

805
flight/OpenPilot/Modules/GPS/GPS.c
View File

@ -31,7 +31,9 @@
#include "openpilot.h"
#include "buffer.h"
#include "GPS.h"
#include "positionactual.h"
#include <stdbool.h>
#include "NMEA.h"
#include "gpsposition.h"
#include "homelocation.h"
#include "WorldMagModel.h"
#include "CoordinateConversions.h"
@ -39,42 +41,13 @@
// constants/macros/typdefs
#define NMEA_BUFFERSIZE 128
// Message Codes
#define NMEA_NODATA 0 // No data. Packet not available, bad, or not decoded
#define NMEA_GPGGA 1 // Global Positioning System Fix Data
#define NMEA_GPVTG 2 // Course over ground and ground speed
#define NMEA_GPGLL 3 // Geographic position - latitude/longitude
#define NMEA_GPGSV 4 // GPS satellites in view
#define NMEA_GPGSA 5 // GPS DOP and active satellites
#define NMEA_GPRMC 6 // Recommended minimum specific GPS data
#define NMEA_UNKNOWN 0xFF// Packet received but not known
#define GPS_TIMEOUT_MS 500
// Debugging
//#define GPSDEBUG
#ifdef GPSDEBUG
#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_VTG ///< define to enable debug of VTG messages
#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages
#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
#endif
// Private functions
static void gpsTask(void* parameters);
static void setHomeLocation(PositionActualData * gpsData);
// functions
char* nmeaGetPacketBuffer(void);
char nmeaChecksum(char* gps_buffer);
uint8_t nmeaProcess(cBuffer* rxBuffer);
void nmeaProcessGPGGA(char* packet);
void nmeaProcessGPRMC(char* packet);
void nmeaProcessGPVTG(char* packet);
void nmeaProcessGPGSA(char* packet);
static bool GPS_copy_sentence_from_cbuffer (char * dest, uint32_t dest_len, cBuffer * rxBuffer);
static void gpsTask(void* parameters);
static void setHomeLocation(GPSPositionData * gpsData);
// Global variables
@ -86,14 +59,16 @@ void nmeaProcessGPGSA(char* packet);
// Private types
// Private variables
static uint8_t gpsPort;
static uint8_t gpsPort;
static xTaskHandle gpsTaskHandle;
cBuffer gpsRxBuffer;
static char gpsRxData[512];
char NmeaPacket[NMEA_BUFFERSIZE];
static uint32_t numUpdates;
static uint32_t numErrors;
static uint32_t timeOfLastUpdateMs;
static cBuffer gpsRxBuffer;
static char gpsRxData[512];
static char NmeaPacket[NMEA_BUFFERSIZE];
static uint32_t timeOfLastUpdateMs;
static uint32_t numUpdates;
static uint32_t numChecksumErrors;
static uint32_t numParsingErrors;
/**
* Initialise the gps module
@ -102,23 +77,18 @@ static uint32_t timeOfLastUpdateMs;
*/
int32_t GPSInitialize(void)
{
signed portBASE_TYPE xReturn;
signed portBASE_TYPE xReturn;
// Init vars
numUpdates = 0;
numErrors = 0;
timeOfLastUpdateMs = 0;
// TODO: Get gps settings object
gpsPort = PIOS_COM_GPS;
// TODO: Get gps settings object
gpsPort = PIOS_COM_GPS;
// Init input buffer size 512
bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
// Init input buffer size 512
bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
// Start gps task
xReturn = xTaskCreate(gpsTask, (signed char*)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle);
// Start gps task
xReturn = xTaskCreate(gpsTask, (signed char*)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle);
return 0;
return 0;
}
/**
@ -126,61 +96,153 @@ int32_t GPSInitialize(void)
*/
static void gpsTask(void* parameters)
{
int32_t gpsRxOverflow=0;
char c;
portTickType xDelay = 100 / portTICK_RATE_MS;
PositionActualData GpsData;
uint32_t timeNowMs;
int32_t gpsRxOverflow = 0;
char c;
portTickType xDelay = 100 / portTICK_RATE_MS;
GPSPositionData GpsData;
uint32_t timeNowMs;
// Loop forever
while(1)
{
/* This blocks the task until there is something on the buffer */
while(PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
c=PIOS_COM_ReceiveBuffer(gpsPort);
if( !bufferAddToEnd(&gpsRxBuffer, c) )
{
// no space in buffer
// count overflow
gpsRxOverflow++;
break;
}
nmeaProcess(&gpsRxBuffer);
}
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ( (timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS )
{
PositionActualGet(&GpsData);
GpsData.Status = POSITIONACTUAL_STATUS_NOGPS;
PositionActualSet(&GpsData);
}
else {
// Loop forever
while (1) {
/* This blocks the task until there is something on the buffer */
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) {
c = PIOS_COM_ReceiveBuffer(gpsPort);
if (!bufferAddToEnd(&gpsRxBuffer, c)) {
/*
* The buffer is already full and we haven't found a valid NMEA sentence.
* Flush the buffer and note the overflow event.
*/
gpsRxOverflow++;
bufferFlush (&gpsRxBuffer);
} else {
/* Grab the next available complete NMEA sentence from the Rx buffer */
if (GPS_copy_sentence_from_cbuffer (NmeaPacket, sizeof(NmeaPacket), &gpsRxBuffer)) {
/* Validate the checksum over the sentence */
if (!NMEA_checksum (NmeaPacket)) {
/* Invalid checksum. May indicate dropped characters on Rx. */
++numChecksumErrors;
} else {
/* Valid checksum, use this packet to update the GPS position */
if (!NMEA_update_position (NmeaPacket)) {
++numParsingErrors;
} else {
++numUpdates;
}
timeOfLastUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS;
}
}
}
}
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS) {
GPSPositionGet(&GpsData);
GpsData.Status = GPSPOSITION_STATUS_NOGPS;
GPSPositionSet(&GpsData);
} else {
// Had an update
HomeLocationData home;
HomeLocationGet(&home);
PositionActualGet(&GpsData);
if( (GpsData.Status == POSITIONACTUAL_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE) ) {
GPSPositionGet(&GpsData);
if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) &&
(home.Set == HOMELOCATION_SET_FALSE)) {
setHomeLocation(&GpsData);
}
}
// Block task until next update
vTaskDelay(xDelay);
}
// Block task until next update
vTaskDelay(xDelay);
}
}
static void setHomeLocation(PositionActualData * gpsData)
static bool GPS_copy_sentence_from_cbuffer (char * dest, uint32_t dest_len, cBuffer * rxBuffer)
{
/* Throw away all initial characters from the Rx buffer that are not the NMEA start flag '$' */
bool startFlag = false;
while (rxBuffer->datalength && !startFlag) {
if (bufferGetAtIndex (rxBuffer, 0) == '$') {
startFlag = true;
} else {
bufferGetFromFront(rxBuffer);
}
}
if (!startFlag) {
/* No start of sentence located, bail out */
return false;
}
/* rxBuffer is now positioned at the start of sentence marker */
/* Start of sentence located, look for an end of sentence marker '\r\n' */
bool endFlag = false;
uint16_t end_pos;
for (end_pos = 2; end_pos < rxBuffer->datalength; end_pos++) {
// check for end of NMEA sentence '\r\n'
if ((bufferGetAtIndex(rxBuffer, end_pos-1) == '\r') &&
(bufferGetAtIndex(rxBuffer, end_pos) == '\n')) {
/* Found the end of the packet */
endFlag = true;
}
}
if (!endFlag) {
/* No end of sentence located, bail out and wait for more data */
return false;
}
/*
* Our rxBuffer must look like this now:
* [0] = '$'
* ... = zero or more bytes of sentence payload
* [end_pos - 1] = '\r'
* [end_pos] = '\n'
*
* Prepare to consume the sentence from the buffer
*/
uint32_t payload_len = end_pos - 2; /* not counting the '$' or '\r\n' */
/* Drop the start flag '$' */
(void) bufferGetFromFront (rxBuffer);
bool truncated = false;
while (payload_len--) {
if (dest_len > 1) {
/* Put the payload data into the buffer as long as there is room */
*dest = bufferGetFromFront (rxBuffer);
dest++;
dest_len--;
} else {
/*
* No more room in the dest buffer. Note that the sentence was truncated and
* continue to flush the rxBuffer.
*/
truncated = true;
(void) bufferGetFromFront (rxBuffer);
}
}
/* Drop the end marker '\r\n' */
(void) bufferGetFromFront (rxBuffer);
(void) bufferGetFromFront (rxBuffer);
/* NULL terminate the dest buffer. We left 1 byte at the end (see 'dest_len > 1' above) */
*dest = '\0';
return (!truncated);
}
static void setHomeLocation(GPSPositionData * gpsData)
{
HomeLocationData home;
HomeLocationGet(&home);
// Store LLA
home.Latitude = (int32_t) (gpsData->Latitude * 10e6);
home.Longitude = (int32_t) (gpsData->Longitude * 10e6);
home.Altitude = gpsData->GeoidSeparation;
home.Latitude = gpsData->Latitude;
home.Longitude = gpsData->Longitude;
home.Altitude = gpsData->GeoidSeparation;
// Compute home ECEF coordinates and the rotation matrix into NED
double LLA[3] = {(double) home.Latitude / 10e6, (double) home.Longitude / 10e6, (double) home.Altitude};
@ -201,565 +263,6 @@ static void setHomeLocation(PositionActualData * gpsData)
}
char* nmeaGetPacketBuffer(void)
{
return NmeaPacket;
}
/**
* Prosesses NMEA sentence checksum
* \param[in] Buffer for parsed nmea sentence
* \return 0 checksum not valid
* \return 1 checksum valid
*/
char nmeaChecksum(char* gps_buffer)
{
char checksum=0;
char checksum_received=0;
PositionActualData GpsData;
PositionActualGet(&GpsData);
for(int x=0; x<NMEA_BUFFERSIZE; x++)
{
if(gps_buffer[x]=='*')
{
//Parsing received checksum...
checksum_received = strtol(&gps_buffer[x+1],NULL,16);
break;
}
else
{
//XOR the received data...
checksum^=gps_buffer[x];
}
}
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum);
if(checksum == checksum_received)
{
++numUpdates;
timeOfLastUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS;
return 1;
}
else
{
++numErrors;
return 0;
}
}
/**
* Prosesses NMEA sentences
* \param[in] cBuffer for prosessed nmea sentences
* \return Message code for found packet
* \return 0xFF NO packet found
*/
uint8_t nmeaProcess(cBuffer* rxBuffer)
{
uint8_t foundpacket = NMEA_NODATA;
uint8_t startFlag = FALSE;
//u08 data;
uint16_t i,j;
// process the receive buffer
// go through buffer looking for packets
while(rxBuffer->datalength)
{
// look for a start of NMEA packet
if(bufferGetAtIndex(rxBuffer,0) == '$')
{
// found start
startFlag = TRUE;
// when start is found, we leave it intact in the receive buffer
// in case the full NMEA string is not completely received. The
// start will be detected in the next nmeaProcess iteration.
// done looking for start
break;
}
else
bufferGetFromFront(rxBuffer);
}
// if we detected a start, look for end of packet
if(startFlag)
{
for(i=1; i<(rxBuffer->datalength)-1; i++)
{
// check for end of NMEA packet <CR><LF>
if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n'))
{
// have a packet end
// dump initial '$'
bufferGetFromFront(rxBuffer);
// copy packet to NmeaPacket
for(j=0; j<(i-1); j++)
{
// although NMEA strings should be 80 characters or less,
// receive buffer errors can generate erroneous packets.
// Protect against packet buffer overflow
if(j<(NMEA_BUFFERSIZE-1))
NmeaPacket[j] = bufferGetFromFront(rxBuffer);
else
bufferGetFromFront(rxBuffer);
}
// null terminate it
if (j<(NMEA_BUFFERSIZE-1)) {
NmeaPacket[j] = 0;
} else {
NmeaPacket[NMEA_BUFFERSIZE-1] = 0;
}
// dump <CR><LF> from rxBuffer
bufferGetFromFront(rxBuffer);
bufferGetFromFront(rxBuffer);
//DEBUG
#ifdef NMEA_DEBUG_PKT
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",NmeaPacket);
#endif
// found a packet
// done with this processing session
foundpacket = NMEA_UNKNOWN;
break;
}
}
}
if(foundpacket)
{
// check message type and process appropriately
if(!strncmp(NmeaPacket, "GPGGA", 5))
{
// process packet of this type
nmeaProcessGPGGA(NmeaPacket);
// report packet type
foundpacket = NMEA_GPGGA;
}
else if(!strncmp(NmeaPacket, "GPVTG", 5))
{
// process packet of this type
nmeaProcessGPVTG(NmeaPacket);
// report packet type
foundpacket = NMEA_GPVTG;
}
else if(!strncmp(NmeaPacket, "GPGSA", 5))
{
// process packet of this type
nmeaProcessGPGSA(NmeaPacket);
// report packet type
foundpacket = NMEA_GPGSA;
}
else if(!strncmp(NmeaPacket, "GPRMC", 5))
{
// process packet of this type
nmeaProcessGPRMC(NmeaPacket);
// report packet type
foundpacket = NMEA_GPRMC;
}
}
else if(rxBuffer->datalength >= rxBuffer->size)
{
// if we found no packet, and the buffer is full
// we're logjammed, flush entire buffer
bufferFlush(rxBuffer);
}
return foundpacket;
}
/**
* Prosesses NMEA GPGGA sentences
* \param[in] Buffer for parsed nmea GPGGA sentence
*/
void nmeaProcessGPGGA(char* packet)
{
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
char *pEnd;
char token_length=0;
long deg,min,decim;
#ifdef NMEA_DEBUG_GGA
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
#endif
// start parsing just after "GPGGA,"
// attempt to reject empty packets right away
if(packet[6]==',' && packet[7]==',')
return;
if(!nmeaChecksum(packet))
{
// checksum not valid
return;
}
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPGGA header
tokens = strsep(&packet, delimiter);
// get UTC time [hhmmss.sss]
tokens = strsep(&packet, delimiter);
//strcpy(GpsInfo.TimeOfFix,tokens);
// next field: latitude
// get latitude [ddmm.mmmmm]
tokens = strsep(&packet, delimiter);
token_length=strlen(tokens);
deg=strtol (tokens,&pEnd,10);
min=deg%100;
deg=deg/100;
decim=strtol (pEnd+1,NULL,10);
if(token_length==10)// 5 decimal output
{
GpsData.Latitude=deg+(min+decim/100000.0)/60.0; // to decimal degrees
}
else if(token_length==9) // 4 decimal output
{
GpsData.Latitude=deg+(min+decim/10000.0)/60.0; // to decimal degrees
}
else if(token_length==11) // 6 decimal output OPGPS
{
GpsData.Latitude=deg+(min+decim/1000000.0)/60.0; // to decimal degrees
}
// next field: N/S indicator
// correct latitute for N/S
tokens = strsep(&packet, delimiter);
if(tokens[0] == 'S') GpsData.Latitude = -GpsData.Latitude;
// next field: longitude
// get longitude [dddmm.mmmmm]
tokens = strsep(&packet, delimiter);
token_length=strlen(tokens);
deg=strtol (tokens,&pEnd,10);
min=deg%100;
deg=deg/100;
decim=strtol (pEnd+1,NULL,10);
if(token_length==11)// 5 decimal output
{
GpsData.Longitude=deg+(min+decim/100000.0)/60.0; // to decimal degrees
}
else if(token_length==10) // 4 decimal output
{
GpsData.Longitude=deg+(min+decim/10000.0)/60.0; // to decimal degrees
}
else if(token_length==12) // 6 decimal output OPGPS
{
GpsData.Longitude=deg+(min+decim/1000000.0)/60.0; // to decimal degrees
}
// next field: E/W indicator
// correct latitute for E/W
tokens = strsep(&packet, delimiter);
if(tokens[0] == 'W') GpsData.Longitude = -GpsData.Longitude;
// next field: position fix status
// position fix status
// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
// check for good position fix
tokens = strsep(&packet, delimiter);
//if((tokens[0] != '0') || (tokens[0] != 0))
// GpsData.Updates++;
// next field: satellites used
// get number of satellites used in GPS solution
tokens = strsep(&packet, delimiter);
GpsData.Satellites=atoi(tokens);
// next field: HDOP (horizontal dilution of precision)
tokens = strsep(&packet, delimiter);
// next field: altitude
// get altitude (in meters mm.m)
tokens = strsep(&packet, delimiter);
//reuse variables for alt
deg=strtol (tokens,&pEnd,10); // always 0.1m resolution? No
decim=strtol (pEnd+1,NULL,10);
if(1) // OPGPS 3 decimal
GpsData.Altitude=deg+decim/1000.0;
else
GpsData.Altitude=deg+decim/10.0;
// next field: altitude units, always 'M'
tokens = strsep(&packet, delimiter);
// next field: geoid separation
tokens = strsep(&packet, delimiter);
//reuse variables for geoid separation
deg=strtol (tokens,&pEnd,10); // always 0.1m resolution? No
decim=strtol (pEnd+1,NULL,10);
if(1) // OPGPS 3 decimal
GpsData.GeoidSeparation=deg+decim/1000.0;
else
GpsData.GeoidSeparation=deg+decim/10.0;
// next field: separation units
// next field: DGPS age
// next field: DGPS station ID
// next field: checksum
PositionActualSet(&GpsData);
}
/**
* Prosesses NMEA GPRMC sentences
* \param[in] Buffer for parsed nmea GPRMC sentence
*/
void nmeaProcessGPRMC(char* packet)
{
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
char *pEnd;
char token_length=0;
long deg,min,decim;
#ifdef NMEA_DEBUG_RMC
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
#endif
// start parsing just after "GPRMC,"
// attempt to reject empty packets right away
if(packet[6]==',' && packet[7]==',')
return;
if(!nmeaChecksum(packet))
{
// checksum not valid
return;
}
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPRMC header
tokens = strsep(&packet, delimiter);
// get UTC time [hhmmss.sss]
tokens = strsep(&packet, delimiter);
//strcpy(GpsInfo.TimeOfFix,tokens);
// next field: Navigation receiver warning A = OK, V = warning
tokens = strsep(&packet, delimiter);
// next field: latitude
// get latitude [ddmm.mmmmm]
tokens = strsep(&packet, delimiter);
token_length=strlen(tokens);
deg=strtol (tokens,&pEnd,10);
min=deg%100;
deg=deg/100;
decim=strtol (pEnd+1,NULL,10);
/*if(token_length==10)// 5 decimal output
{
GpsData.Latitude=deg+(min+decim/100000.0)/60.0; // to decimal degrees
}
else if(token_length==9) // 4 decimal output
{
GpsData.Latitude=deg+(min+decim/10000.0)/60.0; // to decimal degrees
}
else if(token_length==11) // 6 decimal output OPGPS
{
GpsData.Latitude=deg+(min+decim/1000000.0)/60.0; // to decimal degrees
}*/
// next field: N/S indicator
// correct latitute for N/S
tokens = strsep(&packet, delimiter);
//if(tokens[0] == 'S') GpsData.Latitude = -GpsData.Latitude;
// next field: longitude
// get longitude [dddmm.mmmmm]
tokens = strsep(&packet, delimiter);
token_length=strlen(tokens);
deg=strtol (tokens,&pEnd,10);
min=deg%100;
deg=deg/100;
decim=strtol (pEnd+1,NULL,10);
/*if(token_length==11)// 5 decimal output
{
GpsData.Longitude=deg+(min+decim/100000.0)/60.0; // to decimal degrees
}
else if(token_length==10) // 4 decimal output
{
GpsData.Longitude=deg+(min+decim/10000.0)/60.0; // to decimal degrees
}
else if(token_length==12) // 6 decimal output OPGPS
{
GpsData.Longitude=deg+(min+decim/1000000.0)/60.0; // to decimal degrees
}*/
// next field: E/W indicator
// correct latitute for E/W
tokens = strsep(&packet, delimiter);
//if(tokens[0] == 'W') GpsData.Longitude = -GpsData.Longitude;
// next field: speed (knots)
// get speed in knots
tokens = strsep(&packet, delimiter);
deg=strtol (tokens,&pEnd,10);
decim=strtol (pEnd+1,NULL,10);
GpsData.Groundspeed = (deg+(decim/100.0))*0.51444; //OPGPS style to m/s
// next field: True course
// get True course
tokens = strsep(&packet, delimiter);
deg=strtol (tokens,&pEnd,10);
decim=strtol (pEnd+1,NULL,10);
GpsData.Heading = deg+(decim/100.0); //OPGPS style
// next field: Date of fix
// get Date of fix
tokens = strsep(&packet, delimiter);
// next field: Magnetic variation
// next field: E or W
// next field: checksum
PositionActualSet(&GpsData);
}
/**
* Prosesses NMEA GPVTG sentences
* \param[in] Buffer for parsed nmea GPVTG sentence
*/
void nmeaProcessGPVTG(char* packet)
{
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
#ifdef NMEA_DEBUG_VTG
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
#endif
// start parsing just after "GPVTG,"
// attempt to reject empty packets right away
if(packet[6]==',' && packet[7]==',')
return;
if(!nmeaChecksum(packet))
{
// checksum not valid
return;
}
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPVTG header
tokens = strsep(&packet, delimiter);
// get course (true north ref) in degrees [ddd.dd]
tokens = strsep(&packet, delimiter);
// next field: 'T'
tokens = strsep(&packet, delimiter);
// next field: course (magnetic north)
// get course (magnetic north ref) in degrees [ddd.dd]
tokens = strsep(&packet, delimiter);
// next field: 'M'
tokens = strsep(&packet, delimiter);
// next field: speed (knots)
// get speed in knots
tokens = strsep(&packet, delimiter);
// next field: 'N'
tokens = strsep(&packet, delimiter);
// next field: speed (km/h)
// get speed in km/h
tokens = strsep(&packet, delimiter);
// next field: 'K'
// next field: checksum
PositionActualSet(&GpsData);
}
/**
* Prosesses NMEA GPGSA sentences
* \param[in] Buffer for parsed nmea GPGSA sentence
*/
void nmeaProcessGPGSA(char* packet)
{
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
char *pEnd;
long value,decim;
int mode;
#ifdef NMEA_DEBUG_GSA
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
#endif
// start parsing just after "GPGSA,"
// attempt to reject empty packets right away
if(packet[6]==',' && packet[7]==',')
return;
if(!nmeaChecksum(packet))
{
// checksum not valid
return;
}
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPGSA header
tokens = strsep(&packet, delimiter);
// next field: Mode
// Mode: M=Manual, forced to operate in 2D or 3D, A=Automatic, 3D/2D
tokens = strsep(&packet, delimiter);
// next field: Mode
// Mode: 1=Fix not available, 2=2D, 3=3D
tokens = strsep(&packet, delimiter);
mode = atoi(tokens);
if (mode == 1)
{
GpsData.Status = POSITIONACTUAL_STATUS_NOFIX;
}
else if (mode == 2)
{
GpsData.Status = POSITIONACTUAL_STATUS_FIX2D;
}
else if (mode == 3)
{
GpsData.Status = POSITIONACTUAL_STATUS_FIX3D;
}
// next field: 3-14 IDs of SVs used in position fix (null for unused fields)
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
tokens = strsep(&packet, delimiter);
// next field: PDOP
tokens = strsep(&packet, delimiter);
value=strtol (tokens,&pEnd,10);
decim=strtol (pEnd+1,NULL,10);
GpsData.PDOP=value+decim/100.0;
// next field: HDOP
tokens = strsep(&packet, delimiter);
value=strtol (tokens,&pEnd,10);
decim=strtol (pEnd+1,NULL,10);
GpsData.HDOP=value+decim/100.0;
// next field: VDOP
tokens = strsep(&packet, delimiter);
value=strtol (tokens,&pEnd,10);
decim=strtol (pEnd+1,NULL,10);
GpsData.VDOP=value+decim/100.0;
// next field: checksum
PositionActualSet(&GpsData);
}
/**
* @}

532
flight/OpenPilot/Modules/GPS/NMEA.c Normal file
View File

@ -0,0 +1,532 @@
#include "openpilot.h"
#include "pios.h"
#include "NMEA.h"
#include "gpsposition.h"
// Debugging
//#define GPSDEBUG
#ifdef GPSDEBUG
#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_VTG ///< define to enable debug of VTG messages
#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages
#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
#endif
/* Utility functions */
static float NMEA_real_to_float (char * nmea_real);
static bool NMEA_latlon_to_fixed_point (int32_t * latlon, char * nmea_latlon);
/* NMEA sentence parsers */
struct nmea_parser {
const char * prefix;
bool (*handler)(GPSPositionData * GpsData, char * sentence);
};
static bool nmeaProcessGPGGA (GPSPositionData * GpsData, char * sentence);
static bool nmeaProcessGPRMC (GPSPositionData * GpsData, char * sentence);
static bool nmeaProcessGPVTG (GPSPositionData * GpsData, char * sentence);
static bool nmeaProcessGPGSA (GPSPositionData * GpsData, char * sentence);
static struct nmea_parser nmea_parsers[] = {
{
.prefix = "GPGGA",
.handler = nmeaProcessGPGGA,
},
{
.prefix = "GPVTG",
.handler = nmeaProcessGPVTG,
},
{
.prefix = "GPGSA",
.handler = nmeaProcessGPGSA,
},
{
.prefix = "GPRMC",
.handler = nmeaProcessGPRMC,
},
};
static struct nmea_parser * NMEA_find_parser_by_prefix (char * prefix)
{
if (!prefix) {
return (NULL);
}
for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) {
struct nmea_parser * parser = &nmea_parsers[i];
/* Use strcmp to check for exact equality over the entire prefix */
if (!strcmp(prefix, parser->prefix)) {
/* Found an appropriate parser */
return (parser);
}
}
/* No matching parser for this prefix */
return (NULL);
}
/**
* Computes NMEA sentence checksum
* \param[in] Buffer for parsed nmea sentence
* \return false checksum not valid
* \return true checksum valid
*/
bool NMEA_checksum (char * nmea_sentence)
{
uint8_t checksum_computed = 0;
uint8_t checksum_received;
while (*nmea_sentence != '\0' &&
*nmea_sentence != '*') {
checksum_computed ^= *nmea_sentence;
nmea_sentence++;
}
/* Make sure we're now pointing at the checksum */
if (*nmea_sentence == '\0') {
/* Buffer ran out before we found a checksum marker */
return false;
}
/* Load the checksum from the buffer */
checksum_received = strtol(nmea_sentence + 1, NULL, 16);
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed);
return (checksum_computed == checksum_received);
}
/**
* Parses a complete NMEA sentence and updates the GPSPosition UAVObject
* \param[in] An NMEA sentence with a valid checksum
* \return true if the sentence was successfully parsed
* \return false if any errors were encountered with the parsing
*/
bool NMEA_update_position (char * nmea_sentence)
{
char * sentence = nmea_sentence;
struct nmea_parser * parser;
char * prefix;
/* Find out what kind of NMEA packet we're dealing with */
prefix = strsep(&sentence, ",");
/* Check if we have a parser for this packet type */
parser = NMEA_find_parser_by_prefix (prefix);
if (!parser) {
/* Valid but unhandled packet type */
return false;
}
/* Found a matching parser for this packet type */
/* Reject empty (but valid) packets without parsing */
if (sentence[0] == ',' &&
sentence[1] == ',' &&
sentence[2] == ',') {
/* Nothing to parse, */
return false;
}
/* Parse the sentence and update the GpsData object */
GPSPositionData GpsData;
GPSPositionGet(&GpsData);
if (!parser->handler(&GpsData, sentence)) {
/* Parse failed for valid checksum. Do not update the UAVObject. */
return false;
}
GPSPositionSet(&GpsData);
/* Tell the caller what kind of packet we just parsed */
return true;
}
/**
* Parse an NMEA GPGGA 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 nmeaProcessGPGGA(GPSPositionData * GpsData, char * sentence)
{
char * next = sentence;
char * tokens;
char * delimiter = ",*";
#ifdef NMEA_DEBUG_GGA
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",sentence);
#endif
// get UTC time [hhmmss.sss]
tokens = strsep(&next, delimiter);
//strcpy(GpsInfo.TimeOfFix,tokens);
// 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 latitute 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: position fix status
// position fix status
// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
// check for good position fix
tokens = strsep(&next, delimiter);
//if((tokens[0] != '0') || (tokens[0] != 0))
// GpsData.Updates++;
// next field: satellites used
// get number of satellites used in GPS solution
tokens = strsep(&next, delimiter);
GpsData->Satellites=atoi(tokens);
// next field: HDOP (horizontal dilution of precision)
tokens = strsep(&next, delimiter);
// next field: altitude
// get altitude (in meters mm.m)
tokens = strsep(&next, delimiter);
GpsData->Altitude = NMEA_real_to_float (tokens);
// next field: altitude units, always 'M'
tokens = strsep(&next, delimiter);
// next field: geoid separation
tokens = strsep(&next, delimiter);
GpsData->GeoidSeparation = NMEA_real_to_float (tokens);
// next field: separation units
tokens = strsep(&next, delimiter);
// next field: DGPS age
tokens = strsep(&next, delimiter);
// next field: DGPS station ID
tokens = strsep(&next, delimiter);
// next field: checksum
tokens = strsep(&next, delimiter);
return true;
}
/**
* Parse an NMEA GPRMC 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 nmeaProcessGPRMC (GPSPositionData * GpsData, char * sentence)
{
char * next = sentence;
char * tokens;
char * delimiter = ",*";
#ifdef NMEA_DEBUG_RMC
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",sentence);
#endif
// get UTC time [hhmmss.sss]
tokens = strsep(&next, delimiter);
// next field: Navigation receiver warning A = OK, V = warning
tokens = strsep(&next, delimiter);
// 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 latitute 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 latitute for E/W
tokens = strsep(&next, delimiter);
if(tokens[0] == 'W') GpsData->Longitude = -GpsData->Longitude;
// next field: speed (knots)
// get speed in knots
tokens = strsep(&next, delimiter);
GpsData->Groundspeed = NMEA_real_to_float (tokens);
GpsData->Groundspeed *= 0.51444;
// next field: True course
// get True course
tokens = strsep(&next, delimiter);
GpsData->Heading = NMEA_real_to_float (tokens);
// next field: Date of fix
// get Date of fix
tokens = strsep(&next, delimiter);
// next field: Magnetic variation
tokens = strsep(&next, delimiter);
// next field: E or W
tokens = strsep(&next, delimiter);
// next field: Mode: A=autonomous, D=differential, E=Estimated, N=not valid, S=Simulator
tokens = strsep(&next, delimiter);
// next field: checksum
tokens = strsep(&next, delimiter);
return true;
}
/**
* Parse an NMEA GPVTG 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 nmeaProcessGPVTG (GPSPositionData * GpsData, char * sentence)
{
char * next = sentence;
char * tokens;
char * delimiter = ",*";
#ifdef NMEA_DEBUG_VTG
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",sentence);
#endif
// get course (true north ref) in degrees [ddd.dd]
tokens = strsep(&next, delimiter);
// next field: 'T'
tokens = strsep(&next, delimiter);
// next field: course (magnetic north)
// get course (magnetic north ref) in degrees [ddd.dd]
tokens = strsep(&next, delimiter);
// next field: 'M'
tokens = strsep(&next, delimiter);
// next field: speed (knots)
// get speed in knots
tokens = strsep(&next, delimiter);
// next field: 'N'
tokens = strsep(&next, delimiter);
// next field: speed (km/h)
// get speed in km/h
tokens = strsep(&next, delimiter);
// next field: 'K'
tokens = strsep(&next, delimiter);
// next field: checksum
tokens = strsep(&next, delimiter);
return true;
}
/**
* Parse an NMEA GPGSA 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 nmeaProcessGPGSA (GPSPositionData * GpsData, char * sentence)
{
char * next = sentence;
char * tokens;
char * delimiter = ",*";
#ifdef NMEA_DEBUG_GSA
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",sentence);
#endif
// next field: Mode
// Mode: M=Manual, forced to operate in 2D or 3D, A=Automatic, 3D/2D
tokens = strsep(&next, delimiter);
// next field: Mode
// 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 */
PIOS_DEBUG_Assert(0);
break;
}
// next field: 3-14 IDs of SVs used in position fix (null for unused fields)
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
tokens = strsep(&next, delimiter);
// next field: PDOP
tokens = strsep(&next, delimiter);
GpsData->PDOP = NMEA_real_to_float (tokens);
// next field: HDOP
tokens = strsep(&next, delimiter);
GpsData->HDOP = NMEA_real_to_float (tokens);
// next field: VDOP
tokens = strsep(&next, delimiter);
GpsData->VDOP = NMEA_real_to_float (tokens);
// 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.
* The fract_units field indicates the units of the fractional part as
* 1 whole = 10^fract_units fract
*/
static inline bool NMEA_parse_real (int32_t * whole, uint32_t * fract, uint8_t * fract_units, char * field)
{
char * s = field;
char * field_w;
char * field_f;
PIOS_DEBUG_Assert (whole);
PIOS_DEBUG_Assert (fract);
PIOS_DEBUG_Assert (fract_units);
field_w = strsep(&s, ".");
field_f = s;
*whole = strtol (field_w, NULL, 10);
if (field_w) {
/* decimal was found so we may have a fractional part */
*fract = strtoul (field_f, NULL, 10);
*fract_units = strlen(field_f);
} else {
/* no decimal was found, fractional part is zero */
*fract = 0;
*fract_units = 0;
}
return true;
}
/*
* This function only exists to deal with a linking
* failure in the stdlib function strtof(). This
* implementation does not rely on the _sbrk() syscall
* like strtof() does.
*/
float NMEA_real_to_float (char * nmea_real)
{
int32_t whole;
uint32_t fract;
uint8_t fract_units;
/* Sanity checks */
PIOS_DEBUG_Assert (nmea_real);
if (! NMEA_parse_real (&whole, &fract, &fract_units, nmea_real)) {
return false;
}
/* Convert to float */
return (((float)whole) + fract * pow(10, -fract_units));
}
/*
* Parse a field in the format:
* DD[D]MM.mmmm[mm]
* into a fixed-point representation in units of (degrees * 1e-7)
*/
bool NMEA_latlon_to_fixed_point (int32_t * latlon, char * nmea_latlon)
{
int32_t num_DDDMM;
uint32_t num_m;
uint8_t units;
/* Sanity checks */
PIOS_DEBUG_Assert (nmea_latlon);
PIOS_DEBUG_Assert (latlon);
if (! NMEA_parse_real (&num_DDDMM, &num_m, &units, nmea_latlon)) {
return false;
}
/* scale up the mmmm[mm] field apropriately depending on # of digits */
switch (units) {
case 0:
/* no digits, value is zero so no scaling */
break;
case 4: /* mmmm */
num_m *= 1e3; /* mmmm000 */
break;
case 5: /* mmmmm */
num_m *= 1e2; /* mmmmm00 */
break;
case 6: /* mmmmmm */
num_m *= 1e1; /* mmmmmm0 */
break;
default:
/* unhandled format */
PIOS_DEBUG_Assert(0);
break;
}
*latlon = (num_DDDMM / 100) * 1e7; /* scale the whole degrees */
*latlon += (num_DDDMM % 100) * 1e7 / 60; /* add in the scaled decimal whole minutes */
*latlon += num_m / 60; /* add in the scaled decimal fractional minutes */
return true;
}

10
flight/OpenPilot/Modules/GPS/inc/NMEA.h Normal file
View File

@ -0,0 +1,10 @@
#ifndef NMEA_H
#define NMEA_H
#include <stdbool.h>
#include <stdint.h>
extern bool NMEA_update_position (char * nmea_sentence);
extern bool NMEA_checksum (char * nmea_sentence);
#endif /* NMEA_H */