gps: gpsd: Provide GPRMC sensor for backwards compatibility.

This commit introduces the "gps_gprmc" sensor and exposes it via
the property tree.

Signed-off-by: Moritz Fischer <moritz.fischer@ettus.com>

1 files changed, 64 insertions, 1 deletions

diff --git a/host/lib/usrp/gpsd_iface.cpp b/host/lib/usrp/gpsd_iface.cpp
index 62b6939c5..5dbeebb97 100644
--- a/host/lib/usrp/gpsd_iface.cpp
+++ b/host/lib/usrp/gpsd_iface.cpp
@@ -64,7 +64,7 @@ public:
         _bthread.swap(t);
 
 
-        _sensors = boost::assign::list_of("gps_locked")("gps_time")("gps_position");
+        _sensors = boost::assign::list_of("gps_locked")("gps_time")("gps_position")("gps_gprmc");
     }
 
     virtual ~gpsd_iface_impl(void)
@@ -90,6 +90,9 @@ public:
         } else if (key == "gps_time") {
             return sensor_value_t(
                 "GPS epoch time", int(_epoch_time()), "seconds");
+        } else if (key == "gps_gprmc") {
+            return sensor_value_t(
+                "GPRMC", _gps_gprmc(), "");
         } else if (key == "gps_position") {
             return sensor_value_t(
                 "GPS Position", str(
@@ -164,6 +167,66 @@ private: // member functions
         return tmp;
     }
 
+    float _zeroize(float x)
+    {
+      return std::isnan(x) ? 0.0 : x;
+    }
+
+    int _nmea_checksum(const std::string &s)
+    {
+        if ((s.at(0) != '$'))
+            return 0;
+
+        boost::uint8_t sum = '\0';
+        for (size_t i = 1; i < s.size(); i++)
+            sum ^= static_cast<boost::uint8_t>(s.at(i));
+
+        return sum;
+    }
+
+    double _deg_to_dm(double angle)
+    {
+        double fraction, integer;
+        fraction = std::modf(angle, &integer);
+        return std::floor(angle) * 100 + fraction * 60;
+    }
+
+    std::string _gps_gprmc(void)
+    {
+        struct tm tm;
+        time_t intfixtime;
+
+        boost::shared_lock<boost::shared_mutex> l(_d_mutex);
+
+        tm.tm_mday = tm.tm_mon = tm.tm_year = 0;
+        tm.tm_hour = tm.tm_min = tm.tm_sec = 0;
+
+        if (std::isnan(_gps_data.fix.time) == 0) {
+            intfixtime = (time_t) _gps_data.fix.time;
+            (void)gmtime_r(&intfixtime, &tm);
+            tm.tm_mon++;
+            tm.tm_year %= 100;
+        }
+        std::string string = str(boost::format(
+           "$GPRMC,%02d%02d%02d,%c,%09.4f,%c,%010.4f,%c,%.4f,%.3f,%02d%02d%02d,,")
+        % tm.tm_hour
+        % tm.tm_min
+        % tm.tm_sec
+        % (_gps_data.status ? 'A' : 'V')
+        % _zeroize(_deg_to_dm(std::fabs(_gps_data.fix.latitude)))
+        % ((_gps_data.fix.latitude > 0) ? 'N' : 'S')
+        % _zeroize(_deg_to_dm(std::fabs(_gps_data.fix.longitude)))
+        % ((_gps_data.fix.longitude > 0) ? 'E' : 'W')
+        % _zeroize(_gps_data.fix.speed * MPS_TO_KNOTS)
+        % _zeroize(_gps_data.fix.track)
+        % tm.tm_mday % tm.tm_mon % tm.tm_year);
+
+        string.append(str(
+            boost::format("*%02X") % _nmea_checksum(string)));
+
+        return string;
+    }
+
 private: // members
     std::vector<std::string> _sensors;
     bool                     _detected;