mpm: e310: Enable GPS sensors

The GPS sensors are pulled in via gpsd_iface.

2 files changed, 70 insertions, 5 deletions

diff --git a/mpm/python/usrp_mpm/gpsd_iface.py b/mpm/python/usrp_mpm/gpsd_iface.py
index cbd43f0a6..72cde22b1 100644
--- a/mpm/python/usrp_mpm/gpsd_iface.py
+++ b/mpm/python/usrp_mpm/gpsd_iface.py
@@ -191,18 +191,26 @@ class GPSDIfaceExtension(object):
     """
     def __init__(self):
         self._gpsd_iface = GPSDIface()
-        self._gpsd_iface.open()
         self._log = self._gpsd_iface.log
+        self._initialized = False
+        try:
+            self._gpsd_iface.open()
+            self._initialized = True
+        except (ConnectionRefusedError, ConnectionResetError):
+            self._log.warning(
+                "Could not connect to GPSd! None of the GPS sensors will work!")
 
     def __del__(self):
-        self._gpsd_iface.close()
+        if self._initialized:
+            self._gpsd_iface.close()
 
     def extend(self, context):
         """Register the GSPDIfaceExtension object's public function with `context`"""
         new_methods = [method_name for method_name in dir(self)
                        if not method_name.startswith('_') \
                        and callable(getattr(self, method_name)) \
-                       and method_name != "extend"]
+                       and method_name.endswith("sensor") \
+                       and self._initialized]
         for method_name in new_methods:
             new_method = getattr(self, method_name)
             self._log.trace("%s: Adding %s method", context, method_name)
@@ -377,6 +385,26 @@ class GPSDIfaceExtension(object):
             'value': gpgga,
         }
 
+    def get_gps_lock(self):
+        """
+        Get the GPS lock status using the TPV data.
+
+        The Jackson Labs GPS modules have a pin to query GPS lock, which is a
+        better option.
+        """
+        if not self._initialized:
+            self._log.warning("Cannot query GPS lock, GPSd not initialized!")
+            return False
+        # Read responses from GPSD until we get a non-trivial mode
+        while True:
+            gps_info = self._gpsd_iface.get_gps_info(resp_class='tpv', timeout=15)
+            self._log.trace("GPS info: {}".format(gps_info))
+            if gps_info.get("mode", 0) > 0:
+                break
+        # 2 == 2D fix, 3 == 3D fix.
+        # https://gpsd.gitlab.io/gpsd/gpsd_json.html
+        return gps_info.get("mode", 0) >= 2
+
 
 def main():
     """Test functionality of the GPSDIface class"""
@@ -401,8 +429,6 @@ def main():
     gps_ext = GPSDIfaceExtension()
     for _ in range(10):
         print(gps_ext.get_gps_time_sensor().get('value'))
-    #TODO Add GPSDIfaceExtension code
-
 
 if __name__ == "__main__":
     main()
diff --git a/mpm/python/usrp_mpm/periph_manager/e31x.py b/mpm/python/usrp_mpm/periph_manager/e31x.py
index 93e404658..c729ceef1 100644
--- a/mpm/python/usrp_mpm/periph_manager/e31x.py
+++ b/mpm/python/usrp_mpm/periph_manager/e31x.py
@@ -10,9 +10,11 @@ E310 implementation module
 
 from __future__ import print_function
 import copy
+import re
 from six import itervalues
 from usrp_mpm.components import ZynqComponents
 from usrp_mpm.dboard_manager import E31x_db
+from usrp_mpm.gpsd_iface import GPSDIfaceExtension
 from usrp_mpm.mpmtypes import SID
 from usrp_mpm.mpmutils import assert_compat_number, str2bool
 from usrp_mpm.periph_manager import PeriphManagerBase
@@ -70,6 +72,7 @@ class e31x(ZynqComponents, PeriphManagerBase):
     mboard_info = {"type": "e3xx"}
     mboard_sensor_callback_map = {
         'ref_locked': 'get_ref_lock_sensor',
+        'gps_locked': 'get_gps_lock_sensor',
         'temp_fpga' : 'get_fpga_temp_sensor',
         'temp_mb' : 'get_mb_temp_sensor',
     }
@@ -146,6 +149,7 @@ class e31x(ZynqComponents, PeriphManagerBase):
         """
         self._clock_source = None
         self._time_source = None
+        self._gpsd = None
         self.dboards = []
         self.dboard = None
         self.mboard_regs_control = None
@@ -181,6 +185,29 @@ class e31x(ZynqComponents, PeriphManagerBase):
             # an application tries to use the device.
             self.apply_idle_overlay()
             self._device_initialized = False
+        self._init_gps_sensors()
+
+    def _init_gps_sensors(self):
+        """
+        Init and register the GPSd Iface and related sensor functions
+
+        Note: The GPS chip is not connected to the FPGA, so this is initialized
+        regardless of the idle state
+        """
+        self.log.trace("Initializing GPSd interface")
+        self._gpsd = GPSDIfaceExtension()
+        new_methods = self._gpsd.extend(self)
+        for method_name in new_methods:
+            try:
+                # Extract the sensor name from the getter
+                sensor_name = re.search(r"get_(.*)_sensor", method_name).group(1)
+                # Register it with the MB sensor framework
+                self.mboard_sensor_callback_map[sensor_name] = method_name
+                self.log.trace("Adding %s sensor function", sensor_name)
+            except AttributeError:
+                # re.search will return None is if can't find the sensor name
+                self.log.warning("Error while registering sensor function: %s", method_name)
+
 
     def _init_normal(self):
         """
@@ -596,6 +623,18 @@ class e31x(ZynqComponents, PeriphManagerBase):
             'value': str(lock_status).lower(),
         }
 
+    def get_gps_lock_sensor(self):
+        """
+        Get lock status of GPS as a sensor dict
+        """
+        gps_locked = self._gpsd.get_gps_lock()
+        return {
+            'name': 'gps_lock',
+            'type': 'BOOLEAN',
+            'unit': 'locked' if gps_locked else 'unlocked',
+            'value': str(gps_locked).lower(),
+        }
+
     def get_mb_temp_sensor(self):
         """
         Get temperature sensor reading of the E310.