diff options
| -rw-r--r-- | mpm/python/usrp_mpm/gpsd_iface.py | 101 |
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""" |