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

handling HCHDG and HCHDT heading packets
qgis · Jan 6, 2020 · 494b52b · 494b52b
1 parent 58f0e81
commit 494b52b
Show file tree

Hide file tree

Showing 6 changed files with 149 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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