From 494b52b23151cf60ecb83a4a7a81a7883ec602e4 Mon Sep 17 00:00:00 2001 From: Nyall Dawson Date: Mon, 6 Jan 2020 13:16:42 +1000 Subject: [PATCH] [gps] Sync some changes from a fork of nmealib, and add support for handling HCHDG and HCHDT heading packets --- external/nmea/parse.c | 76 ++++++++++++++++++- external/nmea/parse.h | 4 +- external/nmea/sentence.h | 26 ++++++- .../gps/qgsnmeaconnection.sip.in | 8 ++ src/core/gps/qgsnmeaconnection.cpp | 36 +++++++++ src/core/gps/qgsnmeaconnection.h | 4 + 6 files changed, 149 insertions(+), 5 deletions(-) diff --git a/external/nmea/parse.c b/external/nmea/parse.c index cdd019872ca..8ca72d9eae6 100644 --- a/external/nmea/parse.c +++ b/external/nmea/parse.c @@ -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. diff --git a/external/nmea/parse.h b/external/nmea/parse.h index 5982fe2c4ac..b30d8f4c305 100644 --- a/external/nmea/parse.h +++ b/external/nmea/parse.h @@ -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 ); diff --git a/external/nmea/sentence.h b/external/nmea/sentence.h index 72b028b1c0a..76d5da8e685 100644 --- a/external/nmea/sentence.h +++ b/external/nmea/sentence.h @@ -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 ); diff --git a/python/core/auto_generated/gps/qgsnmeaconnection.sip.in b/python/core/auto_generated/gps/qgsnmeaconnection.sip.in index 6e765332e37..257d87ca355 100644 --- a/python/core/auto_generated/gps/qgsnmeaconnection.sip.in +++ b/python/core/auto_generated/gps/qgsnmeaconnection.sip.in @@ -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 }; diff --git a/src/core/gps/qgsnmeaconnection.cpp b/src/core/gps/qgsnmeaconnection.cpp index 6afd7e3604f..ab320af8832 100644 --- a/src/core/gps/qgsnmeaconnection.cpp +++ b/src/core/gps/qgsnmeaconnection.cpp @@ -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; diff --git a/src/core/gps/qgsnmeaconnection.h b/src/core/gps/qgsnmeaconnection.h index cee503742a2..79d591efeac 100644 --- a/src/core/gps/qgsnmeaconnection.h +++ b/src/core/gps/qgsnmeaconnection.h @@ -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