mpm: Add gpgga sensor function to GPSd iface

Add method to generate GPGGA sensor data in MPM devices. This needs to
be constructed from TPV and SKY sensor data, and matches the GPGGA
sensor functionality in gpsd_iface.cpp.

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"""