Skip to content

GitLab

Commit d9cf54dd authored by Guillaume Gonnet's avatar Guillaume Gonnet
Browse files

Finish GPS parser.

parent ee520636
Hide whitespace changes
Inline Side-by-side
Showing with 323 additions and 513 deletions
src/app/drivers/gps-parser.c 0 → 100644
View file @ d9cf54dd
/*
Parse GPS data.
Copyright (C) 2019, ENSIMAG students
This file is based on LoRaMAC-node from Semtech, under Revised BSD License.
*/
#include "gps.h"
#include <string.h>
#include <stdlib.h>
// Various type of NMEA data we can receive with the GPS.
static const char NmeaDataTypeGPGGA[] = "GPGGA";
static const char NmeaDataTypeGPRMC[] = "GPRMC";
// Value used for the conversion of the position from DMS to decimal.
static const int32_t MaxNorthPosition = 8388607; // 2^23 - 1
static const int32_t MaxSouthPosition = 8388608; // -2^23
static const int32_t MaxEastPosition = 8388607; // 2^23 - 1
static const int32_t MaxWestPosition = 8388608; // -2^23
// GPS data in ASCII and numrical formats.
gps_nmea_t gps_nmea;
gps_data_t gps_data;
// Convert a nibble to hex char.
static int8_t nibble_to_hex(uint8_t a)
{
if (a < 10)
return '0' + a;
else if (a < 16)
return 'A' + (a - 10);
else
return '?';
}
// Convert GPS positions from double to binary values.
static void positions_to_binary(void)
{
long double temp;
if (gps_data.latitude >= 0) { // North
temp = gps_data.latitude * MaxNorthPosition;
gps_data.latitude_bin = temp / 90;
} else { // South
temp = gps_data.latitude * MaxSouthPosition;
gps_data.latitude_bin = temp / 90;
}
if (gps_data.longitude >= 0) { // East
temp = gps_data.longitude * MaxEastPosition;
gps_data.longitude_bin = temp / 180;
} else { // West
temp = gps_data.longitude * MaxWestPosition;
gps_data.longitude_bin = temp / 180;
}
}
// Convert GPS positions from ASCII to double values.
static void positions_to_double(void)
{
double valueTmp1, valueTmp2, valueTmp3, valueTmp4;
// Convert the latitude from ASCII to uint8_t values.
for (int i = 0 ; i < 10 ; i++ )
gps_nmea.latitude[i] = gps_nmea.latitude[i] & 0xF;
// Convert latitude from degree/minute/second (DMS) format into decimal.
valueTmp1 = (double)gps_nmea.latitude[0] * 10.0 + (double)gps_nmea.latitude[1];
valueTmp2 = (double)gps_nmea.latitude[2] * 10.0 + (double)gps_nmea.latitude[3];
valueTmp3 = (double)gps_nmea.latitude[5] * 1000.0 + (double)gps_nmea.latitude[6] * 100.0 +
(double)gps_nmea.latitude[7] * 10.0 + (double)gps_nmea.latitude[8];
gps_data.latitude = valueTmp1 + ((valueTmp2 + (valueTmp3 * 0.0001)) / 60.0);
if (gps_nmea.latitude_pole[0] == 'S')
gps_data.latitude *= -1;
// Convert the longitude from ASCII to uint8_t values.
for (int i = 0 ; i < 10 ; i++)
gps_nmea.longitude[i] = gps_nmea.longitude[i] & 0xF;
// Convert longitude from degree/minute/second (DMS) format into decimal.
valueTmp1 = (double)gps_nmea.longitude[0] * 100.0 + (double)gps_nmea.longitude[1] * 10.0 + (double)gps_nmea.longitude[2];
valueTmp2 = (double)gps_nmea.longitude[3] * 10.0 + (double)gps_nmea.longitude[4];
valueTmp3 = (double)gps_nmea.longitude[6] * 1000.0 + (double)gps_nmea.longitude[7] * 100;
valueTmp4 = (double)gps_nmea.longitude[8] * 10.0 + (double)gps_nmea.longitude[9];
gps_data.longitude = valueTmp1 + (valueTmp2 / 60.0) + (((valueTmp3 + valueTmp4) * 0.0001) / 60.0);
if (gps_nmea.longitude_pole[0] == 'W')
gps_data.longitude *= -1;
}
// Calculates the checksum for a NMEA sentence (and return the position of the
// checksum in the sentence).
static int32_t nmea_checksum(int8_t *nmeaStr, int32_t nmeaStrSize, int8_t *checksum)
{
int i = 0;
uint8_t checkNum = 0;
if ((nmeaStr == NULL) || (checksum == NULL) || (nmeaStrSize <= 1))
return -1;
if (nmeaStr[i] == '$')
i += 1; // Skip the first '$' if necessary.
while (nmeaStr[i] != '*') {
checkNum ^= nmeaStr[i]; // XOR until '*' or max length is reached.
i += 1;
if (i >= nmeaStrSize)
return -1;
}
// Convert checksum value to 2 hexadecimal characters.
checksum[0] = nibble_to_hex(checkNum / 16); // upper nibble
checksum[1] = nibble_to_hex(checkNum % 16); // lower nibble
return i + 1;
}
// Calculate the checksum of a NMEA frame and compare it to the checksum that
// is present at the end of it (return true if it matches).
static bool nmea_validate_checksum(int8_t *serialBuff, int32_t buffSize)
{
int32_t checksumIndex;
int8_t checksum[2]; // 2 characters to calculate NMEA checksum.
checksumIndex = nmea_checksum(serialBuff, buffSize, checksum);
if (checksumIndex < 0)
return false;
if (checksumIndex >= (buffSize - 2))
return false; // Not enough char in the serial buffer to read checksum.
return // Check the checksum.
(serialBuff[checksumIndex + 0] == checksum[0]) &&
(serialBuff[checksumIndex + 1] == checksum[1]);
}
// Format GPS data.
static void format_gps_data(void)
{
positions_to_double();
positions_to_binary();
if (gps_data.has_fix)
gps_data.altitude = atoi(gps_nmea.altitude);
}
// Read a field from RX buffer.
#define READ_FIELD(field, i, rxBuffer, maxSize) \
{ \
uint8_t __fs = 0; \
while ((rxBuffer)[(i) + __fs++] != ',') \
if (__fs > (maxSize)) return GPS_FAIL; \
for (uint8_t __j = 0; __j < __fs; __j++, (i)++) \
(field)[__j] = (rxBuffer)[i]; \
}
// Read a field from RX buffer.
#define SKIP_FIELD(i, rxBuffer, maxSize) \
{ \
uint8_t __fs = 0; \
while ((rxBuffer)[(i) + __fs++] != ',') \
if (__fs > (maxSize)) return GPS_FAIL; \
(i) += __fs; \
}
// Parse a GPGGA message.
static uint8_t parse_GPGGA(uint8_t i, int8_t *rxBuffer)
{
// NmeaUtcTime.
SKIP_FIELD(i, rxBuffer, 11);
// NmeaLatitude.
READ_FIELD(gps_nmea.latitude, i, rxBuffer, 10);
// NmeaLatitudePole.
READ_FIELD(gps_nmea.latitude_pole, i, rxBuffer, 2);
// NmeaLongitude.
READ_FIELD(gps_nmea.longitude, i, rxBuffer, 11);
// NmeaLongitudePole.
READ_FIELD(gps_nmea.longitude_pole, i, rxBuffer, 2);
// NmeaFixQuality.
READ_FIELD(gps_nmea.fix_quality, i, rxBuffer, 2);
// NmeaSatelliteTracked.
SKIP_FIELD(i, rxBuffer, 3);
// NmeaHorizontalDilution.
SKIP_FIELD(i, rxBuffer, 6);
// NmeaAltitude.
READ_FIELD(gps_nmea.altitude, i, rxBuffer, 8);
// NmeaAltitudeUnit.
SKIP_FIELD(i, rxBuffer, 2);
// NmeaHeightGeoid.
SKIP_FIELD(i, rxBuffer, 8);
// NmeaHeightGeoidUnit.
SKIP_FIELD(i, rxBuffer, 2);
gps_data.has_fix = (gps_nmea.fix_quality[0] > 0x30);
format_gps_data();
return GPS_SUCCESS;
}
// Parse a GPRMC message.
static uint8_t parse_GPRMC(uint8_t i, int8_t *rxBuffer)
{
// NmeaUtcTime.
SKIP_FIELD(i, rxBuffer, 11);
// NmeaDataStatus.
READ_FIELD(gps_nmea.fix_quality, i, rxBuffer, 2);
// NmeaLatitude.
READ_FIELD(gps_nmea.latitude, i, rxBuffer, 10);
// NmeaLatitudePole.
READ_FIELD(gps_nmea.latitude_pole, i, rxBuffer, 2);
// NmeaLongitude.
READ_FIELD(gps_nmea.longitude, i, rxBuffer, 11);
// NmeaLongitudePole.
READ_FIELD(gps_nmea.longitude_pole, i, rxBuffer, 2);
// NmeaSpeed.
SKIP_FIELD(i, rxBuffer, 8);
// NmeaDetectionAngle.
SKIP_FIELD(i, rxBuffer, 8);
// NmeaDate.
SKIP_FIELD(i, rxBuffer, 8);
gps_data.has_fix = (gps_nmea.fix_quality[0] == 0x41);
format_gps_data();
return GPS_SUCCESS;
}
// Parse GPS data.
uint8_t gps_parse_data(int8_t *rxBuffer, int32_t rxBufferSize)
{
if (!nmea_validate_checksum(rxBuffer, rxBufferSize))
return GPS_FAIL;
uint8_t i = 1;
READ_FIELD(gps_nmea.data_type, i, rxBuffer, 6);
if (strncmp(gps_nmea.data_type, NmeaDataTypeGPGGA, 5) == 0)
return parse_GPGGA(i, rxBuffer);
else if (strncmp(gps_nmea.data_type, NmeaDataTypeGPRMC, 5) == 0)
return parse_GPRMC(i, rxBuffer);
else
return GPS_FAIL;
}
// Reset GPS data.
void gps_reset_data(void)
{
gps_data.has_fix = false;
gps_data.altitude = 0xFFFF;
gps_data.latitude = 0;
gps_data.longitude = 0;
gps_data.latitude_bin = 0;
gps_data.longitude_bin = 0;
}
src/app/drivers/gps.c
View file @ d9cf54dd
......@@ -7,517 +7,39 @@ This file is based on LoRaMAC-node from Semtech, under Revised BSD License.
*/
#include "gps.h"
#include "app.h"
uint8_t gps_parse_data(void)
{
// TODO: limit update.
return 0;
}
#define TRIGGER_GPS_CNT 10
// Various type of NMEA data we can receive with the GPS.
static const char NmeaDataTypeGPGGA[] = "GPGGA";
static const char NmeaDataTypeGPRMC[] = "GPRMC";
// Value used for the conversion of the position from DMS to decimal.
static const int32_t MaxNorthPosition = 8388607; // 2^23 - 1
static const int32_t MaxSouthPosition = 8388608; // -2^23
static const int32_t MaxEastPosition = 8388607; // 2^23 - 1
static const int32_t MaxWestPosition = 8388608; // -2^23
gps_nmea_t gps_nmea;
// Parsed data in double and binary formats.
static double HasFix = false;
static double latitude = 0;
static double longitude = 0;
static int32_t latitude_bin = 0;
static int32_t longitude_bin = 0;
static int16_t altitude = 0xFFFF;
// Mutex that protects GPS data.
static mutex_t gps_mutex = MUTEX_INIT;
// Get the lastest GPS position in binary format.
uint8_t gps_position_binary(int32_t *latiBin, int32_t *longiBin)
{
// TODO: lock mutex
uint8_t status = HasFix ? GPS_SUCCESS : GPS_FAIL;
if (!HasFix)
GpsResetPosition();
*latiBin = LatitudeBinary;
*longiBin = LongitudeBinary;
return status;
}
// Get the lastest GPS altitude.
int16_t gps_altitude(void)
{
// TODO: lock mutex
BoardDisableIrq( );
if( HasFix == true )
{
Altitude = atoi( NmeaGpsData.NmeaAltitude );
}
else
{
Altitude = 0xFFFF;
}
BoardEnableIrq( );
return Altitude;
}
void GpsConvertPositionIntoBinary(void)
{
long double temp;
if (latitude >= 0) { // North
temp = latitude * MaxNorthPosition;
latitude_bin = temp / 90;
}
else { // South
temp = latitude * MaxSouthPosition;
latitude_bin = temp / 90;
}
if (longitude >= 0) { // East
temp = longitude * MaxEastPosition;
longitude_bin = temp / 180;
}
else { // West
temp = longitude * MaxWestPosition;
longitude_bin = temp / 180;
}
}
// GPS parsed data.
extern gps_data_t gps_data;
// Convert GPS positions from ASCII to double values.
static void positions_to_double(void)
uint8_t gps_parse_data2(void)
{
double valueTmp1, valueTmp2, valueTmp3, valueTmp4;
// Convert the latitude from ASCII to uint8_t values.
for (int i = 0 ; i < 10 ; i++ )
gps_nmea.latitude[i] = gps_nmea.latitude[i] & 0xF;
// Convert latitude from degree/minute/second (DMS) format into decimal.
valueTmp1 = (double)gps_nmea.latitude[0] * 10.0 + (double)gps_nmea.latitude[1];
valueTmp2 = (double)gps_nmea.latitude[2] * 10.0 + (double)gps_nmea.latitude[3];
valueTmp3 = (double)gps_nmea.latitude[5] * 1000.0 + (double)gps_nmea.latitude[6] * 100.0 +
(double)gps_nmea.latitude[7] * 10.0 + (double)gps_nmea.latitude[8];
latitude = valueTmp1 + ((valueTmp2 + (valueTmp3 * 0.0001)) / 60.0);
if (gps_nmea.latitude_pole[0] == 'S')
latitude *= -1;
// Convert the longitude from ASCII to uint8_t values.
for (int i = 0 ; i < 10 ; i++)
gps_nmea.longitude[i] = gps_nmea.longitude[i] & 0xF;
// Convert longitude from degree/minute/second (DMS) format into decimal.
valueTmp1 = (double)gps_nmea.longitude[0] * 100.0 + (double)gps_nmea.longitude[1] * 10.0 + (double)gps_nmea.longitude[2];
valueTmp2 = (double)gps_nmea.longitude[3] * 10.0 + (double)gps_nmea.longitude[4];
valueTmp3 = (double)gps_nmea.longitude[6] * 1000.0 + (double)gps_nmea.longitude[7] * 100;
valueTmp4 = (double)gps_nmea.longitude[8] * 10.0 + (double)gps_nmea.longitude[9];
longitude = valueTmp1 + (valueTmp2 / 60.0) + (((valueTmp3 + valueTmp4) * 0.0001) / 60.0);
if (gps_nmea.longitude_pole[0] == 'W')
Longitude *= -1;
}
// Calculates the checksum for a NMEA sentence (and return the position of the
// checksum in the sentence).
static int32_t nmea_checksum(int8_t *nmeaStr, int32_t nmeaStrSize, int8_t *checksum)
{
int i = 0;
uint8_t checkNum = 0;
if ((nmeaStr == NULL) || (checksum == NULL) || (nmeaStrSize <= 1))
return -1;
if (nmeaStr[i] == '$')
i += 1; // Skip the first '$' if necessary.
while (nmeaStr[i] != '*') {
checkNum ^= nmeaStr[i]; // XOR until '*' or max length is reached.
i += 1;
if (i >= nmeaStrSize)
return -1;
}
// Convert checksum value to 2 hexadecimal characters.
checksum[0] = Nibble2HexChar(checkNum / 16); // upper nibble
checksum[1] = Nibble2HexChar(checkNum % 16); // lower nibble
// TODO: limit update.
return i + 1;
return 0;
}
// Calculate the checksum of a NMEA frame and compare it to the checksum that
// is present at the end of it (return true if it matches).
static bool nmea_validate_checksum(int8_t *serialBuff, int32_t buffSize)
{
int32_t checksumIndex;
int8_t checksum[2]; // 2 characters to calculate NMEA checksum.
checksumIndex = nmea_checksum(serialBuff, buffSize, checksum);
if (checksumIndex < 0)
return false;
if (checksumIndex >= (buffSize - 2))
return false; // Not enough char in the serial buffer to read checksum.
return // Check the checksum.
(serialBuff[checksumIndex + 0] == checksum[0]) &&
(serialBuff[checksumIndex + 1] == checksum[1]);
}
// TODO: rename this function.
uint8_t GpsParseGpsData( int8_t *rxBuffer, int32_t rxBufferSize )
// Get the lastest GPS position in binary format.
uint8_t gps_get_binary(int32_t *lat, int32_t *lon, int16_t *alt)
{
uint8_t i = 1;
uint8_t j = 0;
if (!nmea_validate_checksum(rxBuffer, rxBufferSize))
return GPS_FAIL;
uint8_t fieldSize = 0;
while (rxBuffer[i + fieldSize++] != ',') {
if (fieldSize > 6)
return GPS_FAIL;
}
mutex_lock(&gps_mutex);
for (j = 0; j < fieldSize; j++, i++)
gps_nmea.data_type[j] = rxBuffer[i];
uint8_t status = gps_data.has_fix ? GPS_SUCCESS : GPS_FAIL;
if (!gps_data.has_fix)
gps_reset_data();
// Parse the GPGGA data.
if (strncmp(gps_nmea.data_type, NmeaDataTypeGPGGA, 5) == 0)
{
// NmeaUtcTime
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 11 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaUtcTime[j] = rxBuffer[i];
}
// NmeaLatitude
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 10 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaLatitude[j] = rxBuffer[i];
}
// NmeaLatitudePole
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 2 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaLatitudePole[j] = rxBuffer[i];
}
// NmeaLongitude
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 11 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaLongitude[j] = rxBuffer[i];
}
// NmeaLongitudePole
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 2 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaLongitudePole[j] = rxBuffer[i];
}
// NmeaFixQuality
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 2 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaFixQuality[j] = rxBuffer[i];
}
// NmeaSatelliteTracked
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 3 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaSatelliteTracked[j] = rxBuffer[i];
}
// NmeaHorizontalDilution
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 6 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaHorizontalDilution[j] = rxBuffer[i];
}
// NmeaAltitude
fieldSize = 0;
while( rxBuffer[i + fieldSize++] != ',' )
{
if( fieldSize > 8 )
{
return FAIL;
}
}
for( j = 0; j < fieldSize; j++, i++ )
{
gps_nmea.NmeaAltitude[j] = rxBuffer[i];