1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
|
#
# Copyright 2017 Ettus Research, a National Instruments Company
#
# SPDX-License-Identifier: GPL-3.0-or-later
#
"""
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 not result.get(resp_class, {}):
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()
|
