Skip to content
Permalink
Browse files

[gps] Sync some changes from a fork of nmealib, and add support for

handling HCHDG and HCHDT heading packets
  • Loading branch information
nyalldawson committed Jan 6, 2020
1 parent 58f0e81 commit 494b52b23151cf60ecb83a4a7a81a7883ec602e4
@@ -133,12 +133,14 @@ int nmea_pack_type( const char *buff, int buff_sz )
"GPGSV",
"GPRMC",
"GPVTG",
"HCHDG",
"HCHDT",
"GNRMC",
"GPGST",
};

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

NMEA_ASSERT( buff );

@@ -155,8 +157,12 @@ int nmea_pack_type( const char *buff, int buff_sz )
else if ( 0 == memcmp( buff, P_HEADS[4], buffer_size ) )
return GPVTG;
else if ( 0 == memcmp( buff, P_HEADS[5], buffer_size ) )
return GPRMC;
return HCHDG;
else if ( 0 == memcmp( buff, P_HEADS[6], buffer_size ) )
return HCHDT;
else if ( 0 == memcmp( buff, P_HEADS[7], buffer_size ) )
return GPRMC;
else if ( 0 == memcmp( buff, P_HEADS[8], buffer_size ) )
return GPGST;

return GPNON;
@@ -508,6 +514,72 @@ int nmea_parse_GPVTG( const char *buff, int buff_sz, nmeaGPVTG *pack )
return 1;
}

/**
* \brief Parse HCHDG 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_HCHDG( const char *buff, int buff_sz, nmeaHCHDG *pack )
{
NMEA_ASSERT( buff && pack );

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

nmea_trace_buff( buff, buff_sz );

if ( 5 != nmea_scanf( buff, buff_sz,
"$HCHDG,%f,%f,%C,%f,%C*",
&( pack->mag_heading ), &( pack->mag_deviation ),
&( pack->ew_deviation ), &( pack->mag_variation ),
&( pack->ew_variation ) ) )
{
nmea_error( "HCHDG parse error!" );
return 0;
}

if ( pack->ew_deviation != 'E' && pack->ew_deviation != 'W' )
{
nmea_error( "HCHDG invalid deviation direction" );
return 0;
}

if ( pack->ew_variation != 'E' && pack->ew_variation != 'W' )
{
nmea_error( "HCHDG invalid variation direction" );
return 0;
}

return 1;
}

/**
* \brief Parse HDT 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_HCHDT( const char *buff, int buff_sz, nmeaHCHDT *pack )
{
NMEA_ASSERT( buff && pack );

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

nmea_trace_buff( buff, buff_sz );

if ( 2 != nmea_scanf( buff, buff_sz,
"$HCHDT,%f,%C*",
&( pack->direction ), &( pack->t_flag ) ) )
{
nmea_error( "HCHDT parse error!" );
return 0;
}

return 1;
}

/**
* \brief Fill nmeaINFO structure by GGA packet data.
* @param pack a pointer of packet structure.
@@ -38,11 +38,13 @@ 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 );
int nmea_parse_HCHDG( const char *buff, int buff_sz, nmeaHCHDG *pack );
int nmea_parse_HCHDT( const char *buff, int buff_sz, nmeaHCHDT *pack );
int nmea_parse_GPGST( const char *buff, int buff_sz, nmeaGPGST *pack );
int nmea_parse_GPHDT( const char *buff, int buff_sz, nmeaGPHDT *pack );

void nmea_GPGGA2info( nmeaGPGGA *pack, nmeaINFO *info );
@@ -31,7 +31,9 @@ enum nmeaPACKTYPE
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.
GPGST = 0x0012 //!< GST - GPS Pseudorange Noise Statistics
GPGST = 0x0012, //!< GST - GPS Pseudorange Noise Statistics
HCHDG = 0x0020, //!< HDG - Heading, Deviation and Variation
HCHDT = 0x0100, //!< HDT - Heading reference to true north
};

/**
@@ -139,10 +141,30 @@ typedef struct _nmeaGPVTG
typedef struct _nmeaGPHDT
{
double heading; //!< Heading in degrees
char dir_t; //!< Fixed text 'T' indicates that heading is relative to true north
char t_flag; //!< Fixed text 'T' indicates that heading is relative to true north

} nmeaGPHDT;

/**
* HCHDG packet information structure (magnetic heading)
*/
typedef struct _nmeaHCHDG
{
double mag_heading; //!< Magnetic sensor heading (degrees)
double mag_deviation; //!< Magnetic deviation (degrees)
char ew_deviation; //!< [E]ast or [W]est
double mag_variation; //!< Magnetic variation (degrees)
char ew_variation; //!< [E]ast or [W]est
} nmeaHCHDG;

/**
* HDT packet information structure (Heading, )
*/
typedef struct _nmeaHCHDT
{
double direction; //!< Heading respect to true north (degrees)
char t_flag; //!< Static text [T]
} nmeaHCHDT;

void nmea_zero_GPGGA( nmeaGPGGA *pack );
void nmea_zero_GPGST( nmeaGPGST *pack );
@@ -66,6 +66,14 @@ process GST sentence
void processHdtSentence( const char *data, int len );
%Docstring
process HDT sentence
%End
void processHchdgSentence( const char *data, int len );
%Docstring
process HCHDG sentence
%End
void processHchdtSentence( const char *data, int len );
%Docstring
process HCHDT sentence
%End
};

@@ -143,6 +143,20 @@ void QgsNmeaConnection::processStringBuffer()
mStatus = GPSDataReceived;
QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
}
else if ( substring.startsWith( QLatin1String( "$HCHDG" ) ) )
{
QgsDebugMsgLevel( substring, 2 );
processHchdgSentence( ba.data(), ba.length() );
mStatus = GPSDataReceived;
QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
}
else if ( substring.startsWith( QLatin1String( "$HCHDT" ) ) )
{
QgsDebugMsgLevel( substring, 2 );
processHchdtSentence( ba.data(), ba.length() );
mStatus = GPSDataReceived;
QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
}
else
{
QgsDebugMsgLevel( QStringLiteral( "unknown nmea sentence: %1" ).arg( substring ), 2 );
@@ -205,6 +219,28 @@ void QgsNmeaConnection::processHdtSentence( const char *data, int len )
}
}

void QgsNmeaConnection::processHchdgSentence( const char *data, int len )
{
nmeaHCHDG result;
if ( nmea_parse_HCHDG( data, len, &result ) )
{
mLastGPSInformation.direction = result.mag_heading;
if ( result.ew_variation == 'E' )
mLastGPSInformation.direction += result.mag_variation;
else
mLastGPSInformation.direction -= result.mag_variation;
}
}

void QgsNmeaConnection::processHchdtSentence( const char *data, int len )
{
nmeaHCHDT result;
if ( nmea_parse_HCHDT( data, len, &result ) )
{
mLastGPSInformation.direction = result.direction;
}
}

void QgsNmeaConnection::processRmcSentence( const char *data, int len )
{
nmeaGPRMC result;
@@ -60,6 +60,10 @@ class CORE_EXPORT QgsNmeaConnection: public QgsGpsConnection
void processGstSentence( const char *data, int len );
//! process HDT sentence
void processHdtSentence( const char *data, int len );
//! process HCHDG sentence
void processHchdgSentence( const char *data, int len );
//! process HCHDT sentence
void processHchdtSentence( const char *data, int len );
};

#endif // QGSNMEACONNECTION_H

0 comments on commit 494b52b

Please sign in to comment.
You can’t perform that action at this time.