Skip to content

Commit

Permalink
Add NMEA GST Message for GPSInformation.Accuracies (#30274)
Browse files Browse the repository at this point in the history 
* Update sentence.h

* Update sentence.h

* Update sentence.h

* Update sentence.c

* Update parse.h

* Update parse.h

* Update sentence.h

* Update parse.c

* Update qgsnmeaconnection.cpp

* Update qgsnmeaconnection.cpp

* Update qgsnmeaconnection.cpp

* Update info.h

* Update qgsnmeaconnection.h

* Update qgsnmeaconnection.h

* Update parse.c

* Update parse.c

* Update parse.c

- Corrected Buffer Sizecomparison to 6
- defined Variable for buffer_size

* Astyle

* Fix docstrings
  • Loading branch information
@elpaso
elpaso committed Jun 25, 2019
1 parent 710a4fb commit 063d29b
Show file tree
Hide file tree
Showing 8 changed files with 142 additions and 8 deletions.
7 changes: 7 additions & 0 deletions external/nmea/info.h
View file
Expand Up @@ -115,6 +115,13 @@ typedef struct _nmeaINFO
double speed; //!< Speed over the ground in kilometers/hour
double direction; //!< Track angle in degrees True
double declination; //!< Magnetic variation degrees (Easterly var. subtracts from true course)
double rms_pr; //!< RMS value of the pseudorange residuals;
double err_major; //!< Error ellipse semi-major axis 1 sigma error, in meters
double err_minor; //!< Error ellipse semi-minor axis 1 sigma error, in meters
double err_ori; //!< Error ellipse orientation, degrees from true north
double sig_lat; //!< Latitude 1 sigma error, in meters
double sig_lon; //!< Longitude 1 sigma error, in meters
double sig_alt; //!< Height 1 sigma error, in meters

nmeaSATINFO satinfo; //!< Satellites information

Expand Down
80 changes: 73 additions & 7 deletions external/nmea/parse.c
View file
Expand Up @@ -134,24 +134,30 @@ int nmea_pack_type( const char *buff, int buff_sz )
"GPRMC",
"GPVTG",
"GNRMC",
"GPGST",
};

// BUFFER_SIZE = size(P_HEADS) - 1;
int buffer_size = 6;

NMEA_ASSERT( buff );

if ( buff_sz < 5 )
if ( buff_sz < buffer_size )
return GPNON;
else if ( 0 == memcmp( buff, P_HEADS[0], 5 ) )
else if ( 0 == memcmp( buff, P_HEADS[0], buffer_size ) )
return GPGGA;
else if ( 0 == memcmp( buff, P_HEADS[1], 5 ) )
else if ( 0 == memcmp( buff, P_HEADS[1], buffer_size ) )
return GPGSA;
else if ( 0 == memcmp( buff, P_HEADS[2], 5 ) )
else if ( 0 == memcmp( buff, P_HEADS[2], buffer_size ) )
return GPGSV;
else if ( 0 == memcmp( buff, P_HEADS[3], 5 ) )
else if ( 0 == memcmp( buff, P_HEADS[3], buffer_size ) )
return GPRMC;
else if ( 0 == memcmp( buff, P_HEADS[4], 5 ) )
else if ( 0 == memcmp( buff, P_HEADS[4], buffer_size ) )
return GPVTG;
else if ( 0 == memcmp( buff, P_HEADS[5], 5 ) )
else if ( 0 == memcmp( buff, P_HEADS[5], buffer_size ) )
return GPRMC;
else if ( 0 == memcmp( buff, P_HEADS[6], buffer_size ) )
return GPGST;

return GPNON;
}
Expand Down Expand Up @@ -207,6 +213,7 @@ int nmea_find_tail( const char *buff, int buff_sz, int *res_crc )
return nread;
}


/**
* \brief Parse GGA packet from buffer.
* @param buff a constant character pointer of packet buffer.
Expand Down Expand Up @@ -244,6 +251,42 @@ int nmea_parse_GPGGA( const char *buff, int buff_sz, nmeaGPGGA *pack )
return 1;
}

/**
* \brief Parse GST packet from buffer.
* @param buff a constant character pointer of packet buffer.
* @param buff_sz buffer size.
* @param pack a pointer of packet which will filled by function.
* @return 1 (true) - if parsed successfully or 0 (false) - if fail.
*/
int nmea_parse_GPGST( const char *buff, int buff_sz, nmeaGPGST *pack )
{
char time_buff[NMEA_TIMEPARSE_BUF];

NMEA_ASSERT( buff && pack );

memset( pack, 0, sizeof( nmeaGPGST ) );

nmea_trace_buff( buff, buff_sz );

if ( 8 != nmea_scanf( buff, buff_sz,
"$GPGST,%s,%f,%f,%f,%f,%f,%f,%f*",
&( time_buff[0] ),
&( pack->rms_pr ), &( pack->err_major ), &( pack->err_minor ), &( pack->err_ori ),
&( pack->sig_lat ), &( pack->sig_lon ), &( pack->sig_alt ) ) )
{
nmea_error( "GPGST parse error!" );
return 0;
}

if ( 0 != _nmea_parse_time( &time_buff[0], ( int )strlen( &time_buff[0] ), &( pack->utc ) ) )
{
nmea_error( "GPGST time parse error!" );
return 0;
}

return 1;
}

/**
* \brief Parse GSA packet from buffer.
* @param buff a constant character pointer of packet buffer.
Expand Down Expand Up @@ -426,6 +469,29 @@ void nmea_GPGGA2info( nmeaGPGGA *pack, nmeaINFO *info )
info->smask |= GPGGA;
}

/**
* \brief Fill nmeaINFO structure by GST packet data.
* @param pack a pointer of packet structure.
* @param info a pointer of summary information structure.
*/
void nmea_GPGST2info( nmeaGPGST *pack, nmeaINFO *info )
{
NMEA_ASSERT( pack && info );

info->utc.hour = pack->utc.hour;
info->utc.min = pack->utc.min;
info->utc.sec = pack->utc.sec;
info->utc.msec = pack->utc.msec;
info->rms_pr = pack->rms_pr;
info->err_major = pack->err_major;
info->err_minor = pack->err_minor;
info->err_ori = pack->err_ori;
info->sig_lat = pack->sig_lat;
info->sig_lon = pack->sig_lon;
info->sig_alt = pack->sig_alt;
info->smask |= GPGST;
}

/**
* \brief Fill nmeaINFO structure by GSA packet data.
* @param pack a pointer of packet structure.
Expand Down
2 changes: 2 additions & 0 deletions external/nmea/parse.h
View file
Expand Up @@ -38,12 +38,14 @@ int nmea_pack_type( const char *buff, int buff_sz );
int nmea_find_tail( const char *buff, int buff_sz, int *res_crc );

int nmea_parse_GPGGA( const char *buff, int buff_sz, nmeaGPGGA *pack );
int nmea_parse_GPGST( const char *buff, int buff_sz, nmeaGPGST *pack );
int nmea_parse_GPGSA( const char *buff, int buff_sz, nmeaGPGSA *pack );
int nmea_parse_GPGSV( const char *buff, int buff_sz, nmeaGPGSV *pack );
int nmea_parse_GPRMC( const char *buff, int buff_sz, nmeaGPRMC *pack );
int nmea_parse_GPVTG( const char *buff, int buff_sz, nmeaGPVTG *pack );

void nmea_GPGGA2info( nmeaGPGGA *pack, nmeaINFO *info );
void nmea_GPGST2info( nmeaGPGST *pack, nmeaINFO *info );
void nmea_GPGSA2info( nmeaGPGSA *pack, nmeaINFO *info );
void nmea_GPGSV2info( nmeaGPGSV *pack, nmeaINFO *info );
void nmea_GPRMC2info( nmeaGPRMC *pack, nmeaINFO *info );
Expand Down
6 changes: 6 additions & 0 deletions external/nmea/sentence.c
View file
Expand Up @@ -38,6 +38,12 @@ void nmea_zero_GPGGA( nmeaGPGGA *pack )
pack->diff_units = 'M';
}

void nmea_zero_GPGST( nmeaGPGST *pack )
{
memset( pack, 0, sizeof( nmeaGPGST ) );
nmea_time_now( &pack->utc );
}

void nmea_zero_GPGSA( nmeaGPGSA *pack )
{
memset( pack, 0, sizeof( nmeaGPGSA ) );
Expand Down
20 changes: 19 additions & 1 deletion external/nmea/sentence.h
View file
Expand Up @@ -30,7 +30,8 @@ enum nmeaPACKTYPE
GPGSA = 0x0002, //!< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values.
GPGSV = 0x0004, //!< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values.
GPRMC = 0x0008, //!< RMC - Recommended Minimum Specific GPS/TRANSIT Data.
GPVTG = 0x0010 //!< VTG - Actual track made good and speed over ground.
GPVTG = 0x0010, //!< VTG - Actual track made good and speed over ground.
GPGST = 0x0012 //!< GST - GPS Pseudorange Noise Statistics
};

/**
Expand All @@ -55,6 +56,22 @@ typedef struct _nmeaGPGGA

} nmeaGPGGA;

/**
* GST packet information structure (GPS Pseudorange Noise Statistics)
*/
typedef struct _nmeaGPGST
{
nmeaTIME utc; //!< UTC of position fix
double rms_pr; //!< RMS value of the pseudorange residuals;
double err_major; //!< Error ellipse semi-major axis 1 sigma error, in meters
double err_minor; //!< Error ellipse semi-minor axis 1 sigma error, in meters
double err_ori; //!< Error ellipse orientation, degrees from true north
double sig_lat; //!< Latitude 1 sigma error, in meters
double sig_lon; //!< Longitude 1 sigma error, in meters
double sig_alt; //!< Height 1 sigma error, in meters

} nmeaGPGST;

/**
* GSA packet information structure (Satellite status)
*/
Expand Down Expand Up @@ -117,6 +134,7 @@ typedef struct _nmeaGPVTG
} nmeaGPVTG;

void nmea_zero_GPGGA( nmeaGPGGA *pack );
void nmea_zero_GPGST( nmeaGPGST *pack );
void nmea_zero_GPGSA( nmeaGPGSA *pack );
void nmea_zero_GPGSV( nmeaGPGSV *pack );
void nmea_zero_GPRMC( nmeaGPRMC *pack );
Expand Down
5 changes: 5 additions & 0 deletions python/core/auto_generated/gps/qgsnmeaconnection.sip.in
View file
Expand Up @@ -59,6 +59,11 @@ process VTG sentence
%Docstring
process GSA sentence
%End
void processGstSentence( const char *data, int len );
%Docstring
process GST sentence
%End

};

/************************************************************************
Expand Down
27 changes: 27 additions & 0 deletions src/core/gps/qgsnmeaconnection.cpp
View file
Expand Up @@ -28,6 +28,9 @@
#include "gmath.h"
#include "info.h"

// for sqrt
#include <math.h>

#define KNOTS_TO_KMH 1.852

QgsNmeaConnection::QgsNmeaConnection( QIODevice *device )
Expand Down Expand Up @@ -126,6 +129,13 @@ void QgsNmeaConnection::processStringBuffer()
mStatus = GPSDataReceived;
QgsDebugMsg( QStringLiteral( "*******************GPS data received****************" ) );
}
else if ( substring.startsWith( QLatin1String( "$GPGST" ) ) )
{
QgsDebugMsg( substring );
processGstSentence( ba.data(), ba.length() );
mStatus = GPSDataReceived;
QgsDebugMsg( QStringLiteral( "*******************GPS data received****************" ) );
}
emit nmeaSentenceReceived( substring ); // added to be able to save raw data
}
}
Expand Down Expand Up @@ -158,6 +168,23 @@ void QgsNmeaConnection::processGgaSentence( const char *data, int len )
}
}

void QgsNmeaConnection::processGstSentence( const char *data, int len )
{
nmeaGPGST result;
if ( nmea_parse_GPGST( data, len, &result ) )
{
//update mLastGPSInformation
double sig_lat = result.sig_lat;
double sig_lon = result.sig_lon;
double sig_alt = result.sig_alt;

// Horizontal RMS
mLastGPSInformation.hacc = sqrt( ( pow( sig_lat, 2 ) + pow( sig_lon, 2 ) ) / 2.0 );
// Vertical RMS
mLastGPSInformation.vacc = sig_alt;
}
}

void QgsNmeaConnection::processRmcSentence( const char *data, int len )
{
nmeaGPRMC result;
Expand Down
3 changes: 3 additions & 0 deletions src/core/gps/qgsnmeaconnection.h
View file
Expand Up @@ -56,6 +56,9 @@ class CORE_EXPORT QgsNmeaConnection: public QgsGpsConnection
void processVtgSentence( const char *data, int len );
//! process GSA sentence
void processGsaSentence( const char *data, int len );
//! process GST sentence
void processGstSentence( const char *data, int len );

};

#endif // QGSNMEACONNECTION_H

0 comments on commit 063d29b

Please sign in to comment.