From cb6d78d9e8d4ebda3e31b15868e509711816bfaa Mon Sep 17 00:00:00 2001 From: Brent Stapleton Date: Tue, 5 Dec 2017 12:25:34 -0800 Subject: mpm: adding GPS sensor functions Adding GPS TPV and SKY sensors to MPM, and their respective getters. Reviewed-By: Martin Braun --- mpm/python/usrp_mpm/gpsd_iface.py | 152 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 152 insertions(+) create mode 100644 mpm/python/usrp_mpm/gpsd_iface.py (limited to 'mpm/python/usrp_mpm/gpsd_iface.py') 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() -- cgit v1.2.3