[gpsd-dev] [PATCH 4/6] Add heading and rate of turn NMEA sentences (HDG, ROT)

[chris]

From: Christian Gagneraud <address@hidden>

---
 driver_nmea0183.c |  111 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 111 insertions(+), 0 deletions(-)

diff --git a/driver_nmea0183.c b/driver_nmea0183.c
index a9631cb..d2e45fa 100644
--- a/driver_nmea0183.c
+++ b/driver_nmea0183.c
@@ -770,6 +770,64 @@ static gps_mask_t processGPZDA(int c UNUSED, char *field[],
     return mask;
 }
 
+static gps_mask_t processHDG(int c UNUSED, char *field[],
+                               struct gps_device_t *session)
+{
+    /*
+     * $HCHDG,<1>,<2>,<3>,<4>,<5>*hh
+     *
+     * NMEA 0183 standard Heading, Deviation and Variation.
+     *
+     * <1> Magnetic sensor heading, degrees, to the nearest 0.1 degree.
+     * <2> Magnetic deviation, degrees east or west, to the nearest 0.1 degree.
+     * <3> E if field <2> is degrees East, W if field <2> is degrees West
+     * <4> Magnetic variation, degrees east or west, to the nearest 0.1 degree.
+     * <5> E if field <4> is degrees East, W if field <4> is degrees West
+     */
+    gps_mask_t mask;
+    if ((field[3][0] != 'W' && field[3][0] != 'E') ||
+       (field[5][0] != 'W' && field[5][0] != 'E'))
+        return 0;
+    mask = ONLINE_SET;
+
+    session->gpsdata.attitude.heading = safe_atof(field[1]);
+    if (field[3][0] == 'W')
+      session->gpsdata.attitude.mag_dev = -safe_atof(field[2]);
+    else
+      session->gpsdata.attitude.mag_dev = safe_atof(field[2]);
+    if (field[5][0] == 'W')
+      session->gpsdata.attitude.mag_var = -safe_atof(field[4]);
+    else
+      session->gpsdata.attitude.mag_var = safe_atof(field[4]);
+    session->gpsdata.attitude.rot = NAN;
+    session->gpsdata.attitude.mag_st = '\0';
+    session->gpsdata.attitude.pitch = NAN;
+    session->gpsdata.attitude.pitch_st = '\0';
+    session->gpsdata.attitude.roll = NAN;
+    session->gpsdata.attitude.roll_st = '\0';
+    session->gpsdata.attitude.yaw = NAN;
+    session->gpsdata.attitude.yaw_st = '\0';
+    session->gpsdata.attitude.dip = NAN;
+    session->gpsdata.attitude.mag_len = NAN;
+    session->gpsdata.attitude.mag_x = NAN;
+    session->gpsdata.attitude.mag_y = NAN;
+    session->gpsdata.attitude.mag_z = NAN;
+    session->gpsdata.attitude.acc_len = NAN;
+    session->gpsdata.attitude.acc_x = NAN;
+    session->gpsdata.attitude.acc_y = NAN;
+    session->gpsdata.attitude.acc_z = NAN;
+    session->gpsdata.attitude.gyro_x = NAN;
+    session->gpsdata.attitude.gyro_y = NAN;
+    session->gpsdata.attitude.temp = NAN;
+    session->gpsdata.attitude.depth = NAN;
+    mask |= (ATTITUDE_SET);
+
+    gpsd_report(LOG_RAW, "time %.3f, heading %lf.\n",
+               session->newdata.time,
+               session->gpsdata.attitude.heading);
+    return mask;
+}
+
 static gps_mask_t processHDT(int c UNUSED, char *field[],
                                struct gps_device_t *session)
 {
@@ -807,6 +865,57 @@ static gps_mask_t processHDT(int c UNUSED, char *field[],
     session->gpsdata.attitude.acc_z = NAN;
     session->gpsdata.attitude.gyro_x = NAN;
     session->gpsdata.attitude.gyro_y = NAN;
+    session->gpsdata.attitude.temp = NAN;
+    session->gpsdata.attitude.depth = NAN;
+    mask |= (ATTITUDE_SET);
+
+    gpsd_report(LOG_RAW, "time %.3f, heading %lf.\n",
+               session->newdata.time,
+               session->gpsdata.attitude.heading);
+    return mask;
+}
+
+static gps_mask_t processROT(int c UNUSED, char *field[],
+                               struct gps_device_t *session)
+{
+    /*
+     * $TIROT,<1>,<2>*hh
+     *
+     * NMEA 0183 standard Rate of Turn.
+     *
+     * <1> Signed rate of turn, degrees per minute, to the nearest 0.1 degree.
+     *     Negative values indicate the bow is turning to port.
+     * <2> Status: A = Data Valid; V = Data Invalid.
+     */
+    gps_mask_t mask;
+    if (field[2][0] != 'V')
+        return 0;
+    mask = ONLINE_SET;
+
+    session->gpsdata.attitude.heading = NAN;
+    session->gpsdata.attitude.mag_dev = NAN;
+    session->gpsdata.attitude.mag_var = NAN;
+    session->gpsdata.attitude.rot = safe_atof(field[1]);
+    session->gpsdata.attitude.mag_st = '\0';
+    session->gpsdata.attitude.pitch = NAN;
+    session->gpsdata.attitude.pitch_st = '\0';
+    session->gpsdata.attitude.roll = NAN;
+    session->gpsdata.attitude.roll_st = '\0';
+    session->gpsdata.attitude.yaw = NAN;
+    session->gpsdata.attitude.yaw_st = '\0';
+    session->gpsdata.attitude.dip = NAN;
+    session->gpsdata.attitude.mag_len = NAN;
+    session->gpsdata.attitude.mag_x = NAN;
+    session->gpsdata.attitude.mag_y = NAN;
+    session->gpsdata.attitude.mag_z = NAN;
+    session->gpsdata.attitude.acc_len = NAN;
+    session->gpsdata.attitude.acc_x = NAN;
+    session->gpsdata.attitude.acc_y = NAN;
+    session->gpsdata.attitude.acc_z = NAN;
+    session->gpsdata.attitude.gyro_x = NAN;
+    session->gpsdata.attitude.gyro_y = NAN;
+    session->gpsdata.attitude.temp = NAN;
+    session->gpsdata.attitude.depth = NAN;
     mask |= (ATTITUDE_SET);
 
     gpsd_report(LOG_RAW, "time %.3f, heading %lf.\n",
@@ -1114,7 +1223,9 @@ gps_mask_t nmea_parse(char *sentence, struct gps_device_t 
* session)
        {"VTG", 0,  false, NULL},       /* ignore Velocity Track made Good */
        {"ZDA", 4,  false, processGPZDA},
        {"GBS", 7,  false, processGPGBS},
+        {"HDG", 6,  false, processHDG},
         {"HDT", 1,  false, processHDT},
+        {"ROT", 3,  false, processROT},
        {"DBT", 7,  true,  processDBT},
 #ifdef TNT_ENABLE
        {"PTNTHTM", 9, false, processTNTHTM},
-- 
1.7.0.4