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

OP-25 Flight/GPS: Optimized GPS, buffer overflow fixed, RMC sentence handler

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@660 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
sambas 2010-05-24 19:49:37 +00:00 committed by sambas
parent d678720855
commit ec40618664
1 changed files with 132 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,6 +49,8 @@
#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
#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages
#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
#endif #endif
// Private functions // Private functions
@ -59,6 +61,7 @@ char* nmeaGetPacketBuffer(void);
char nmeaChecksum(char* gps_buffer); char nmeaChecksum(char* gps_buffer);
uint8_t nmeaProcess(cBuffer* rxBuffer); uint8_t nmeaProcess(cBuffer* rxBuffer);
void nmeaProcessGPGGA(char* packet); void nmeaProcessGPGGA(char* packet);
void nmeaProcessGPRMC(char* packet);
void nmeaProcessGPVTG(char* packet); void nmeaProcessGPVTG(char* packet);
void nmeaProcessGPGSA(char* packet); void nmeaProcessGPGSA(char* packet);
@ -74,7 +77,7 @@ void nmeaProcessGPGSA(char* packet);
static COMPortTypeDef gpsPort; static COMPortTypeDef gpsPort;
static xTaskHandle gpsTaskHandle; static xTaskHandle gpsTaskHandle;
cBuffer gpsRxBuffer; cBuffer gpsRxBuffer;
static char gpsRxData[1024]; static char gpsRxData[512];
char NmeaPacket[NMEA_BUFFERSIZE]; char NmeaPacket[NMEA_BUFFERSIZE];
/** /**
@ -89,7 +92,7 @@ int32_t GpsInitialize(void)
gpsPort = COM_USART2; gpsPort = COM_USART2;
// Init input buffer size 512 // Init input buffer size 512
bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 1024); bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
// Start gps task // Start gps task
xReturn = xTaskCreate(gpsTask, (signed char*)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle); xReturn = xTaskCreate(gpsTask, (signed char*)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle);
@ -121,10 +124,8 @@ static void gpsTask(void* parameters)
gpsRxOverflow++; gpsRxOverflow++;
break; break;
} }
//PIOS_COM_SendFormattedStringNonBlocking(COM_USART1, "%d\r", PIOS_COM_ReceiveBufferUsed(gpsPort));
//vTaskDelay(xDelay);
}
nmeaProcess(&gpsRxBuffer); nmeaProcess(&gpsRxBuffer);
}
vTaskDelay(xDelay); vTaskDelay(xDelay);
} }
} }
@ -161,10 +162,17 @@ char nmeaChecksum(char* gps_buffer)
} }
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum); //PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum);
if(checksum == checksum_received) if(checksum == checksum_received)
{
GpsObjectData GpsData;
GpsObjectGet(&GpsData);
GpsData.Updates++;
GpsObjectSet(&GpsData);
return 1; return 1;
}
else else
{
return 0; return 0;
}
} }
/** /**
@ -266,7 +274,7 @@ uint8_t nmeaProcess(cBuffer* rxBuffer)
else if(!strncmp(NmeaPacket, "GPRMC", 5)) else if(!strncmp(NmeaPacket, "GPRMC", 5))
{ {
// process packet of this type // process packet of this type
//nmeaProcessGPRMC(NmeaPacket); nmeaProcessGPRMC(NmeaPacket);
// report packet type // report packet type
foundpacket = NMEA_GPRMC; foundpacket = NMEA_GPRMC;
} }
@ -296,7 +304,7 @@ void nmeaProcessGPGGA(char* packet)
long deg,min,desim; long deg,min,desim;
#ifdef NMEA_DEBUG_GGA #ifdef NMEA_DEBUG_GGA
//PIOS_COM_SendFormattedStringNonBlocking(COM_USART1,"NMEA: %s\r\n",packet); PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
#endif #endif
// start parsing just after "GPGGA," // start parsing just after "GPGGA,"
@ -307,11 +315,10 @@ void nmeaProcessGPGGA(char* packet)
if(!nmeaChecksum(packet)) if(!nmeaChecksum(packet))
{ {
// checksum not valid // checksum not valid
//PIOS_LED_Toggle(LED2);
return; return;
} }
GpsObjectGet(&GpsData);
GpsObjectGet(&GpsData);
// tokenizer for nmea sentence // tokenizer for nmea sentence
//GPGGA header //GPGGA header
tokens = strsep(&packet, delimiter); tokens = strsep(&packet, delimiter);
@ -376,8 +383,8 @@ void nmeaProcessGPGGA(char* packet)
// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
// check for good position fix // check for good position fix
tokens = strsep(&packet, delimiter); tokens = strsep(&packet, delimiter);
if((tokens[0] != '0') || (tokens[0] != 0)) //if((tokens[0] != '0') || (tokens[0] != 0))
GpsData.Updates++; // GpsData.Updates++;
// next field: satellites used // next field: satellites used
// get number of satellites used in GPS solution // get number of satellites used in GPS solution
@ -407,6 +414,117 @@ void nmeaProcessGPGGA(char* packet)
GpsObjectSet(&GpsData); GpsObjectSet(&GpsData);
} }
/**
* Prosesses NMEA GPRMC sentences
* \param[in] Buffer for parsed nmea GPRMC sentence
*/
void nmeaProcessGPRMC(char* packet)
{
GpsObjectData GpsData;
char *tokens;
char *delimiter = ",";
char *pEnd;
char token_length=0;
long deg,min,desim;
#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;
}
GpsObjectGet(&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;
desim=strtol (pEnd+1,NULL,10);
/*if(token_length==10)// 5 desimal output
{
GpsData.Latitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
}
else if(token_length==9) // 4 desimal output
{
GpsData.Latitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
}
else if(token_length==11) // 6 desimal output OPGPS
{
GpsData.Latitude=deg+(min+desim/1000000.0)/60.0; // to desimal 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;
desim=strtol (pEnd+1,NULL,10);
/*if(token_length==11)// 5 desimal output
{
GpsData.Longitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
}
else if(token_length==10) // 4 desimal output
{
GpsData.Longitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
}
else if(token_length==12) // 6 desimal output OPGPS
{
GpsData.Longitude=deg+(min+desim/1000000.0)/60.0; // to desimal 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);
// next field: True course
// get True course
tokens = strsep(&packet, delimiter);
// 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
GpsObjectSet(&GpsData);
}
/** /**
* Prosesses NMEA GPVTG sentences * Prosesses NMEA GPVTG sentences
* \param[in] Buffer for parsed nmea GPVTG sentence * \param[in] Buffer for parsed nmea GPVTG sentence
@ -419,7 +537,7 @@ void nmeaProcessGPVTG(char* packet)
char *delimiter = ","; char *delimiter = ",";
#ifdef NMEA_DEBUG_VTG #ifdef NMEA_DEBUG_VTG
//PIOS_COM_SendFormattedStringNonBlocking(COM_USART1,"NMEA: %s\r\n",packet); PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
#endif #endif
// start parsing just after "GPVTG," // start parsing just after "GPVTG,"
@ -430,7 +548,6 @@ void nmeaProcessGPVTG(char* packet)
if(!nmeaChecksum(packet)) if(!nmeaChecksum(packet))
{ {
// checksum not valid // checksum not valid
//PIOS_LED_Toggle(LED2);
return; return;
} }
GpsObjectGet(&GpsData); GpsObjectGet(&GpsData);
@ -479,7 +596,7 @@ void nmeaProcessGPGSA(char* packet)
long value,desim; long value,desim;
#ifdef NMEA_DEBUG_GSA #ifdef NMEA_DEBUG_GSA
//PIOS_COM_SendFormattedStringNonBlocking(COM_USART1,"NMEA: %s\r\n",packet); PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
#endif #endif
// start parsing just after "GPGSA," // start parsing just after "GPGSA,"
@ -490,7 +607,6 @@ void nmeaProcessGPGSA(char* packet)
if(!nmeaChecksum(packet)) if(!nmeaChecksum(packet))
{ {
// checksum not valid // checksum not valid
//PIOS_LED_Toggle(LED2);
return; return;
} }
GpsObjectGet(&GpsData); GpsObjectGet(&GpsData);