mpm: adding GPS sensor functions

Adding GPS TPV and SKY sensors to MPM, and their respective getters.

Reviewed-By: Martin Braun <martin.braun@ettus.com>

4 files changed, 198 insertions, 0 deletions

diff --git a/mpm/python/usrp_mpm/CMakeLists.txt b/mpm/python/usrp_mpm/CMakeLists.txt
index 81af6eb6b..c0cc24d34 100644
--- a/mpm/python/usrp_mpm/CMakeLists.txt
+++ b/mpm/python/usrp_mpm/CMakeLists.txt
@@ -26,6 +26,7 @@ SET(USRP_MPM_TOP_FILES
     ${CMAKE_CURRENT_SOURCE_DIR}/discovery.py
     ${CMAKE_CURRENT_SOURCE_DIR}/eeprom.py
     ${CMAKE_CURRENT_SOURCE_DIR}/ethtable.py
+    ${CMAKE_CURRENT_SOURCE_DIR}/gpsd_iface.py
     ${CMAKE_CURRENT_SOURCE_DIR}/liberiotable.py
     ${CMAKE_CURRENT_SOURCE_DIR}/mpmlog.py
     ${CMAKE_CURRENT_SOURCE_DIR}/mpmtypes.py
diff --git a/mpm/python/usrp_mpm/__init__.py b/mpm/python/usrp_mpm/__init__.py
index b2b23f494..cc40e2f84 100644
--- a/mpm/python/usrp_mpm/__init__.py
+++ b/mpm/python/usrp_mpm/__init__.py
@@ -27,4 +27,5 @@ from . import dboard_manager
 from . import xports
 from . import cores
 from . import chips
+from . import gpsd_iface
 from .mpmlog import get_main_logger
diff --git a/mpm/python/usrp_mpm/gpsd_iface.py b/mpm/python/usrp_mpm/gpsd_iface.py
new file mode 100644
index 000000000..49a13cc75
--- /dev/null
+++ b/mpm/python/usrp_mpm/gpsd_iface.py
@@ -0,0 +1,152 @@
+#!/usr/bin/python3
+#
+# Copyright 2017 Ettus Research, a National Instruments Company
+#
+# SPDX-License-Identifier: GPL-3.0
+#
+"""
+GPS service daemon (GPSd) interface class
+"""
+import socket
+import json
+import time
+import select
+from usrp_mpm.mpmlog import get_logger
+
+
+class GPSDIface(object):
+    """
+    Interface to the GPS service daemon (GPSd).
+
+    The GPSDIface implementation can be used as a context manager, and GPSD results should be
+    gotten with the get_gps_info() function. This will filter by the GPSD response class
+    (resp_class) and return that class message. If no filter is provided, this function will return
+    the first response (not counting the VERSION message).
+
+    The MPM SKY sensors returns the first available response- there shouldn't be anything tricky
+    about this. The MPM TPV sensor, however, returns an interesting value in the shortest time
+    possible. If the GPSD has received a TPV report since we connected (and sent the start
+    command), this function should return immediately. However, if no report is ready, the function
+    waits until an interesting result is ready and returns that. This is achieved by discarding
+    `mode=0` responses.
+    """
+    def __init__(self):
+        # Make a logger
+        try:
+            self.log = get_logger('GPSDIface')
+        except AssertionError:
+            from usrp_mpm.mpmlog import get_main_logger
+            self.log = get_main_logger('GPSDIface')
+        # Make a socket to connect to GPSD
+        self.gpsd_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+    def __enter__(self):
+        self.open()
+        self.watch_query()
+        return self
+
+    def __exit__(self, exc_type, exc_value, traceback):
+        self.stop_query()
+        self.close()
+        return exc_type is None
+
+    def open(self):
+        """Open the socket to GPSD"""
+        self.gpsd_socket.connect(('localhost', 2947))
+        self.log.trace("Connected to GPSD.")
+
+    def close(self):
+        """Close the socket"""
+        self.gpsd_socket.close()
+        self.log.trace("Closing the connection to GPSD.")
+
+    def watch_query(self):
+        """Send a WATCH command, which starts operation"""
+        query_cmd = b'?WATCH={"enable":true}'
+        self.gpsd_socket.sendall(query_cmd)
+        self.log.trace("Sent query: {}".format(query_cmd))
+
+    def poll_query(self):
+        """Send a POLL command"""
+        query_cmd = b'?POLL;'
+        self.gpsd_socket.sendall(query_cmd)
+        self.log.trace("Sent query: {}".format(query_cmd))
+
+    def stop_query(self):
+        """Send the command to stop operation"""
+        query_cmd = b'?WATCH={"enable":false}'
+        self.gpsd_socket.sendall(query_cmd)
+        self.log.trace("Sent query: {}".format(query_cmd))
+
+    def socket_read_line(self, timeout=60, interval=0.1):
+        """
+        Read from a socket until newline. If there was no newline until the timeout
+        occurs, raise an error. Otherwise, return the line.
+        """
+        line = b''
+        end_time = time.time() + timeout
+        while time.time() < end_time:
+            socket_ready = select.select([self.gpsd_socket], [], [], 0)[0]
+            if socket_ready:
+                next_char = self.gpsd_socket.recv(1)
+                if next_char == b'\n':
+                    return line.decode('ascii')
+                line += next_char
+            else:
+                time.sleep(interval)
+        raise RuntimeError("socket_read_line() exceeded read timeout!")
+
+    def get_gps_info(self, resp_class='', timeout=60):
+        """Convenience function for getting a response which contains a response class"""
+        # Read results until we see one which contains the requested response class, ie TPV or SKY
+        result = {}
+        end_time = time.time() + timeout
+        while resp_class not in result:
+            try:
+                self.poll_query()
+                json_result = self.socket_read_line(timeout=timeout)
+                self.log.trace(
+                    "Received JSON response: {}".format(json_result)
+                )
+                result = json.loads(json_result)
+                self.log.trace(
+                    "Keys in response: {}".format(result.keys())
+                )
+                if (resp_class == "") or (time.time() > end_time):
+                    # If we don't have a response class filter, just return the first response
+                    # or if we timeout
+                    break
+            except json.JSONDecodeError:
+                # If we get an incomplete packet, this will trigger
+                # In this case, just retry
+                continue
+        # Filter the result by resp_class or return the entire thing
+        # In the filtered case, the result contains a list of 'resp_class' responses,
+        # so we need to get one valid one.
+        return result if (resp_class == "") else result.get(resp_class, [{}])[0]
+
+
+def main():
+    """Test functionality of the GPSDIface class"""
+    # Do some setup
+    import argparse
+
+    def parse_args():
+        """Parse the command-line arguments"""
+        parser = argparse.ArgumentParser(description="Read messages from GPSD")
+        parser.add_argument("--timeout", help="Timeout for the GPSD read", default=20)
+        return parser.parse_args()
+
+    args = parse_args()
+
+    with GPSDIface() as gps_iface:
+        result = gps_iface.get_gps_info(resp_class='', timeout=args.timeout)
+        tpv_result = gps_iface.get_gps_info(resp_class='tpv', timeout=args.timeout)
+        sky_result = gps_iface.get_gps_info(resp_class='sky', timeout=args.timeout)
+    print("Sample result: {}".format(result))
+    print("TPV: {}".format(tpv_result))
+    print("SKY: {}".format(sky_result))
+
+
+if __name__ == "__main__":
+    main()
diff --git a/mpm/python/usrp_mpm/periph_manager/n310.py b/mpm/python/usrp_mpm/periph_manager/n310.py
index ed1b5d164..5d69ac956 100644
--- a/mpm/python/usrp_mpm/periph_manager/n310.py
+++ b/mpm/python/usrp_mpm/periph_manager/n310.py
@@ -23,8 +23,10 @@ import os
 import copy
 import shutil
 import subprocess
+import json
 from six import iteritems, itervalues
 from builtins import object
+from usrp_mpm.gpsd_iface import GPSDIface
 from usrp_mpm.periph_manager import PeriphManagerBase
 from usrp_mpm.mpmtypes import SID
 from usrp_mpm.rpc_server import no_rpc
@@ -251,6 +253,8 @@ class n310(PeriphManagerBase):
     mboard_sensor_callback_map = {
         'ref_locked': 'get_ref_lock_sensor',
         'gps_locked': 'get_gps_lock_sensor',
+        'gps_tpv': 'get_gps_tpv_sensor',
+        'gps_sky': 'get_gps_sky_sensor',
     }
     dboard_eeprom_addr = "e0004000.i2c"
     dboard_eeprom_max_len = 64
@@ -652,6 +656,46 @@ class n310(PeriphManagerBase):
             'unit': 'locked' if gps_locked else 'unlocked',
             'value': str(gps_locked).lower(),
         }
+
+    def get_gps_tpv_sensor(self):
+        """
+        Get a TPV response from GPSd as a sensor dict
+        """
+        self.log.trace("Polling GPS TPV results from GPSD")
+        with GPSDIface() as gps_iface:
+            response_mode = 0
+            # Read responses from GPSD until we get a non-trivial mode
+            while response_mode <= 0:
+                gps_info = gps_iface.get_gps_info(resp_class='tpv', timeout=15)
+                self.log.trace("GPS info: {}".format(gps_info))
+                response_mode = gps_info.get("mode", 0)
+
+            # Return the JSON'd results
+            gps_tpv = json.dumps(gps_info)
+            return {
+                'name': 'gps_tpv',
+                'type': 'STRING',
+                'unit': '',
+                'value': gps_tpv,
+            }
+
+    def get_gps_sky_sensor(self):
+        """
+        Get a SKY response from GPSd as a sensor dict
+        """
+        self.log.trace("Polling GPS SKY results from GPSD")
+        with GPSDIface() as gps_iface:
+            # Just get the first SKY result
+            gps_info = gps_iface.get_gps_info(resp_class='sky', timeout=15)
+            # Return the JSON'd results
+            gps_sky = json.dumps(gps_info)
+            return {
+                'name': 'gps_sky',
+                'type': 'STRING',
+                'unit': '',
+                'value': gps_sky,
+            }
+
     ###########################################################################
     # EEPROMs
     ###########################################################################