1 files changed, 101 insertions, 0 deletions
diff --git a/mpm/python/usrp_mpm/gpsd_iface.py b/mpm/python/usrp_mpm/gpsd_iface.py
index 08e9fdd16..e11874d1c 100644
--- a/mpm/python/usrp_mpm/gpsd_iface.py
+++ b/mpm/python/usrp_mpm/gpsd_iface.py
@@ -13,6 +13,8 @@ import json
 import time
 import select
 import datetime
+import math
+import re
 from usrp_mpm.mpmlog import get_logger
 
 
@@ -263,6 +265,105 @@ class GPSDIfaceExtension(object):
             'value': gps_sky,
         }
 
+    def get_gps_gpgga_sensor(self):
+        """Get GPGGA sensor data by parsing TPV and SKY sensor data"""
+        def _deg_to_dm(angle):
+            """Convert a latitude or longitude from degrees to degrees minutes format"""
+            fraction_int_tuple = math.modf(angle)
+            return fraction_int_tuple[1] * 100 + fraction_int_tuple[0] * 60
+
+        def _nmea_checksum(nmea_sentence):
+            """Calculate the checksum for a NMEA data sentence"""
+            checksum = 0
+            if not nmea_sentence.startswith('$'):
+                return checksum
+
+            for character in nmea_sentence[1:]:
+                checksum ^= ord(character)
+
+            return checksum
+
+        self._log.trace("Polling GPS TPV and SKY results from GPSD")
+        # Read responses from GPSD until we get both a SKY response and TPV
+        # response in non-trivial mode
+        while True:
+            gps_info = self._gpsd_iface.get_gps_info(resp_class='', timeout=15)
+            self._log.trace("GPS info: {}".format(gps_info))
+            tpv_sensor_data = gps_info.get('tpv', [{}])[0]
+            sky_sensor_data = gps_info.get('sky', [{}])[0]
+            if tpv_sensor_data and sky_sensor_data and tpv_sensor_data.get("mode", 0) > 0:
+                break
+
+        gpgga = "$GPGGA,"
+
+        if 'time' in tpv_sensor_data:
+            time_formatted = re.subn(r'\d{4}-\d{2}-\d{2}T(\d{2}):(\d{2}):(\d{2}\.?\d*)Z',
+                                     r'\1\2\3,', tpv_sensor_data.get('time'))
+            if time_formatted[1] == 1:
+                gpgga += time_formatted[0]
+            else:
+                gpgga += ","
+        else:
+            gpgga += ","
+
+        if 'lat' in tpv_sensor_data:
+            latitude = tpv_sensor_data.get('lat')
+            latitude_direction = 'N' if latitude > 0 else 'S'
+            latitude = _deg_to_dm(abs(latitude))
+            gpgga += "{:09.4f},{},".format(latitude, latitude_direction)
+        else:
+            gpgga += "0.0,S,"
+
+        if 'lon' in tpv_sensor_data:
+            longitude = tpv_sensor_data['lon']
+            longitude_direction = 'E' if longitude > 0 else 'W'
+            longitude = _deg_to_dm(abs(longitude))
+            gpgga += "{:010.4f},{},".format(longitude, longitude_direction)
+        else:
+            gpgga += "0.0,W,"
+
+        quality = 0
+        if tpv_sensor_data['mode'] > 1:
+            if tpv_sensor_data.get('status') == 2:
+                quality = 2
+            else:
+                quality = 1
+        gpgga += "{:d},".format(quality)
+
+        if 'satellites' in sky_sensor_data:
+            satellites_used = 0
+            for satellite in sky_sensor_data['satellites']:
+                if 'used' in satellite and satellite['used']:
+                    satellites_used += 1
+            gpgga += "{:02d},".format(satellites_used)
+        else:
+            gpgga += ","
+
+        if 'hdop' in sky_sensor_data:
+            gpgga += "{:.2f},".format(sky_sensor_data['hdop'])
+        else:
+            gpgga += ","
+
+        if 'alt' in tpv_sensor_data:
+            gpgga += "{:2f},{},".format(tpv_sensor_data['alt'], 'M')
+        else:
+            gpgga += ",,"
+
+        # separation data is not present in tpv or sky sensor data
+        gpgga += ",,"
+
+        # differential data is not present
+        gpgga += ",,"
+
+        gpgga += "*{:02X}".format(_nmea_checksum(gpgga))
+
+        return {
+            'name': 'gpgga',
+            'type': 'STRING',
+            'unit': '',
+            'value': gpgga,
+        }
+
 
 def main():
     """Test functionality of the GPSDIface class"""