Add GPSD car position support - needs testing!

README.md

@@ -68,7 +68,7 @@ You can then click 'Download Model' in the web interface's setting tab to trigge
 
 
 ## Chase Car Positions
-At the moment Chasemapper supports receiving chase-car positions via either a Serial-attached GPS, or Horus UDP messages. Refer to the configuration file for setup information for these options.
+At the moment Chasemapper supports receiving chase-car positions via either GPSD, a Serial-attached GPS, or Horus UDP messages. Refer to the configuration file for setup information for these options.
 
 This application can also plot your position onto the tracker.habhub.org map, so others can see when you're out balloon chasing. You can also fetch positions of nearby chase cars from Habitat, to see if others are out chasing as well :-) These options can be enabled from the control pane on the left of the web interface, and can also be set within the configuration file. 
 

chasemapper/gpsd.py

@@ -0,0 +1,349 @@
+#!/usr/bin/env python3
+# coding=utf-8
+"""
+GPS3 Code below sourced from https://github.com/wadda/gps3
+and is licensed under the MIT license.
+Modifications made by M.Jessop to make use of logging when reporting errors.
+
+GPS3 (gps3.py) is a Python 2.7-3.5 GPSD interface (http://www.catb.org/gpsd)
+Default host='127.0.0.1', port=2947, gpsd_protocol='json' in two classes.
+
+1) 'GPSDSocket' creates a GPSD socket connection & request/retrieve GPSD output.
+2) 'DataStream' Streamed gpsd JSON data literates it into python dictionaries.
+
+Import          from gps3 import gps3
+Instantiate     gpsd_socket = gps3.GPSDSocket()
+                data_stream = gps3.DataStream()
+Run             gpsd_socket.connect()
+                gpsd_socket.watch()
+Iterate         for new_data in gpsd_socket:
+                    if new_data:
+                        data_stream.unpack(new_data)
+Use                     print('Altitude = ',data_stream.TPV['alt'])
+                        print('Latitude = ',data_stream.TPV['lat'])
+
+Consult Lines 144-ff for Attribute/Key possibilities.
+or http://www.catb.org/gpsd/gpsd_json.html
+
+Run human.py; python[X] human.py [arguments] for a human experience.
+"""
+from __future__ import print_function
+
+import json
+import logging
+import select
+import socket
+import sys
+import time
+from threading import Thread
+
+
+GPSD_HOST = '127.0.0.1'  # gpsd
+GPSD_PORT = 2947  # defaults
+GPSD_PROTOCOL = 'json'  # "
+
+
+class GPSDSocket(object):
+    """Establish a socket with gpsd, by which to send commands and receive data."""
+
+    def __init__(self):
+        self.streamSock = None
+        self.response = None
+
+    def connect(self, host=GPSD_HOST, port=GPSD_PORT):
+        """Connect to a host on a given port.
+        Arguments:
+            host: default host='127.0.0.1'
+            port: default port=2947
+        """
+        for alotta_stuff in socket.getaddrinfo(host, port, 0, socket.SOCK_STREAM):
+            family, socktype, proto, _canonname, host_port = alotta_stuff
+            try:
+                self.streamSock = socket.socket(family, socktype, proto)
+                self.streamSock.connect(host_port)
+                self.streamSock.setblocking(False)
+                return True
+            except (OSError, IOError) as error:
+                logging.error("GPSD - GPSDSocket connect exception: %s" % str(error))
+                return False
+
+    def watch(self, enable=True, gpsd_protocol=GPSD_PROTOCOL, devicepath=None):
+        """watch gpsd in various gpsd_protocols or devices.
+        Arguments:
+            enable: (bool) stream data to socket
+            gpsd_protocol: (str) 'json' | 'nmea' | 'rare' | 'raw' | 'scaled' | 'split24' | 'pps'
+            devicepath: (str) device path - '/dev/ttyUSBn' for some number n or '/dev/whatever_works'
+        Returns:
+            command: (str) e.g., '?WATCH={"enable":true,"json":true};'
+        """
+        # N.B.: 'timing' requires special attention, as it is undocumented and lives with dragons.
+        command = '?WATCH={{"enable":true,"{0}":true}}'.format(gpsd_protocol)
+
+        if gpsd_protocol == 'rare':  # 1 for a channel, gpsd reports the unprocessed NMEA or AIVDM data stream
+            command = command.replace('"rare":true', '"raw":1')
+        if gpsd_protocol == 'raw':  # 2 channel that processes binary data, received data verbatim without hex-dumping.
+            command = command.replace('"raw":true', '"raw",2')
+        if not enable:
+            command = command.replace('true', 'false')  # sets -all- command values false .
+        if devicepath:
+            command = command.replace('}', ',"device":"') + devicepath + '"}'
+
+        return self.send(command)
+
+    def send(self, command):
+        """Ship commands to the daemon
+        Arguments:
+            command: e.g., '?WATCH={{'enable':true,'json':true}}'|'?VERSION;'|'?DEVICES;'|'?DEVICE;'|'?POLL;'
+        """
+        # The POLL command requests data from the last-seen fixes on all active GPS devices.
+        # Devices must previously have been activated by ?WATCH to be pollable.
+        try:
+            self.streamSock.send(bytes(command, encoding='utf-8'))
+        except TypeError:
+            self.streamSock.send(command)  # 2.7 chokes on 'bytes' and 'encoding='
+        except (OSError, IOError) as error:  # MOE, LEAVE THIS ALONE!...for now.
+            logging.error("GPSD - GPS3 send command fail with %s" % str(error))
+
+    def __iter__(self):
+        """banana"""  # <--- for scale
+        return self
+
+    def next(self, timeout=0):
+        """Return empty unless new data is ready for the client.
+        Arguments:
+            timeout: Default timeout=0  range zero to float specifies a time-out as a floating point
+        number in seconds.  Will sit and wait for timeout seconds.  When the timeout argument is omitted
+        the function blocks until at least one file descriptor is ready. A time-out value of zero specifies
+        a poll and never blocks.
+        """
+        try:
+            waitin, _waitout, _waiterror = select.select((self.streamSock,), (), (), timeout)
+            if not waitin: return None
+            else:
+                gpsd_response = self.streamSock.makefile()  # '.makefile(buffering=4096)' In strictly Python3
+                self.response = gpsd_response.readline()
+            return self.response
+
+        except StopIteration as error:
+            logging.error("GPSD - The readline exception in GPSDSocket.next is %s" % str(error))
+
+    __next__ = next  # Workaround for changes in iterating between Python 2.7 and 3
+
+    def close(self):
+        """turn off stream and close socket"""
+        if self.streamSock:
+            self.watch(enable=False)
+            self.streamSock.close()
+        self.streamSock = None
+
+
+class DataStream(object):
+    """Retrieve JSON Object(s) from GPSDSocket and unpack it into respective
+    gpsd 'class' dictionaries, TPV, SKY, etc. yielding hours of fun and entertainment.
+    """
+    packages = {
+        'VERSION': {'release', 'proto_major', 'proto_minor', 'remote', 'rev'},
+        'TPV': {'alt', 'climb', 'device', 'epc', 'epd', 'eps', 'ept', 'epv', 'epx', 'epy', 'lat', 'lon', 'mode', 'speed', 'tag', 'time', 'track'},
+        'SKY': {'satellites', 'gdop', 'hdop', 'pdop', 'tdop', 'vdop', 'xdop', 'ydop'},
+        # Subset of SKY: 'satellites': {'PRN', 'ss', 'el', 'az', 'used'}  # is always present.
+        'GST': {'alt', 'device', 'lat', 'lon', 'major', 'minor', 'orient', 'rms', 'time'},
+        'ATT': {'acc_len', 'acc_x', 'acc_y', 'acc_z', 'depth', 'device', 'dip', 'gyro_x', 'gyro_y', 'heading', 'mag_len', 'mag_st', 'mag_x',
+                'mag_y', 'mag_z', 'pitch', 'pitch_st', 'roll', 'roll_st', 'temperature', 'time', 'yaw', 'yaw_st'},
+        # 'POLL': {'active', 'tpv', 'sky', 'time'},
+        'PPS': {'device', 'clock_sec', 'clock_nsec', 'real_sec', 'real_nsec', 'precision'},
+        'TOFF': {'device', 'clock_sec', 'clock_nsec', 'real_sec', 'real_nsec'},
+        'DEVICES': {'devices', 'remote'},
+        'DEVICE': {'activated', 'bps', 'cycle', 'mincycle', 'driver', 'flags', 'native', 'parity', 'path', 'stopbits', 'subtype'},
+        # 'AIS': {}  # see: http://catb.org/gpsd/AIVDM.html
+        'ERROR': {'message'}}  # TODO: Full suite of possible GPSD output
+
+    def __init__(self):
+        """Potential data packages from gpsd for a generator of class attribute dictionaries"""
+        for package_name, dataset in self.packages.items():
+            _emptydict = {key: 'n/a' for key in dataset}
+            setattr(self, package_name, _emptydict)
+
+        self.DEVICES['devices'] = {key: 'n/a' for key in self.packages['DEVICE']}  # How does multiple listed devices work?
+        # self.POLL = {'tpv': self.TPV, 'sky': self.SKY, 'time': 'n/a', 'active': 'n/a'}
+
+    def unpack(self, gpsd_socket_response):
+        """Sets new socket data as DataStream attributes in those initialised dictionaries
+        Arguments:
+            gpsd_socket_response (json object):
+        Provides:
+        self attribute dictionaries, e.g., self.TPV['lat'], self.SKY['gdop']
+        Raises:
+        AttributeError: 'str' object has no attribute 'keys' when the device falls out of the system
+        ValueError, KeyError: most likely extra, or mangled JSON data, should not happen, but that
+        applies to a lot of things.
+        """
+        try:
+            fresh_data = json.loads(gpsd_socket_response)  # The reserved word 'class' is popped from JSON object class
+            package_name = fresh_data.pop('class', 'ERROR')  # gpsd data package errors are also 'ERROR'.
+            package = getattr(self, package_name, package_name)  # packages are named for JSON object class
+            for key in package.keys():
+                package[key] = fresh_data.get(key, 'n/a')  # Restores 'n/a' if key is absent in the socket response
+
+        except AttributeError:  # 'str' object has no attribute 'keys'
+            logging.error("GPSD Parser - There is an unexpected exception in DataStream.unpack.")
+            return
+
+        except (ValueError, KeyError) as error:
+            logging.error("GPSD Parser - Other Error - %s" % str(error))
+            return
+
+
+class GPSDAdaptor(object):
+    ''' Connect to a GPSD instance, and pass data onto a callback function '''
+
+    def __init__(self,
+        hostname = '127.0.0.1',
+        port = 2947,
+        callback = None):
+        '''
+        Initialize a GPSAdaptor object.
+
+        This class uses the GPSDSocket class to connect to a GPSD instance,
+        and then formats all received data appropriately and passes it on to chasemapper.
+
+        Args:
+            hostname (str): Hostname of where GPSD is listening.
+            port (int): GPSD listen port (default = 2947)
+            callback (function): Callback to pass appropriately formatted dictionary data to.
+        '''
+
+        self.hostname = hostname
+        self.port = port
+        self.callback = callback
+
+
+        self.gpsd_thread_running = False
+        self.gpsd_thread = None
+        self.start()
+
+
+    def start(self):
+        ''' Start the GPSD thread '''
+        if self.gpsd_thread != None:
+            return
+        else:
+            self.gpsd_thread_running = True
+            self.gpsd_thread = Thread(target=self.gpsd_process_thread)
+            self.gpsd_thread.start()
+
+
+
+    def close(self):
+        ''' Stop the GPSD thread. '''
+        self.gpsd_thread_running = False
+        # Wait for the thread to close.
+        if self.gpsd_thread != None:
+            self.gpsd_thread.join()
+
+
+    def send_to_callback(self, data):
+        '''
+        Send the current GPS data snapshot onto the callback function,
+        if one exists.
+        '''
+
+        # Attempt to pass it onto the callback function.
+        if self.callback != None:
+            try:
+                self.callback(data)
+            except Exception as e:
+                traceback.print_exc()
+                logging.error("GPSD - Error Passing data to callback - %s" % str(e))
+
+
+    def gpsd_process_thread(self):
+        ''' Attempt to connect to a GPSD instance, and read position information '''
+        while self.gpsd_thread_running:
+
+            # Attempt to connect.
+            _gpsd_socket = GPSDSocket()
+            _data_stream = DataStream()
+            _success = _gpsd_socket.connect(host = self.hostname, port = self.port)
+
+            # If we could not connect, wait and try again.
+            if not _success:
+                logging.error("GPSD - Connect failed. Waiting 10 seconds before re-trying.")
+                time.sleep(10)
+                continue
+
+            # Start watching for data.
+            _gpsd_socket.watch(gpsd_protocol = 'json')
+            logging.info("GPSD - Connected to GPSD instance at %s" % self.hostname)
+
+            while self.gpsd_thread_running:
+                # We should be getting GPS data every second.
+                # If this isn't the case, we should close the connection and re-connect.
+                _gpsd_data = _gpsd_socket.next(timeout=10)
+
+
+                if _gpsd_data == None or _gpsd_data == '':
+                    logging.error("GPSD - No data received. Attempting to reconnect.")
+
+                    # Break out of this loop back to the connection loop.
+                    break
+                else:
+                    # Attempt to parse the data.
+                    _data_stream.unpack(_gpsd_data)
+
+                    # Extract the Time-Position-Velocity report.
+                    # This will have fields as defined in: http://www.catb.org/gpsd/gpsd_json.html
+                    _TPV = _data_stream.TPV
+                    if _TPV['lat'] == 'n/a' or _TPV['lon'] == 'n/a':
+                        # No position data. Continue.
+                        continue
+                    else:
+                        # Produce output data structure.
+
+                        if _TPV['speed'] != 'n/a':
+                            _speed = _TPV['speed']
+                        else:
+                            _speed = 0.0
+
+                        _gps_state = {
+                            'type': 'GPS',
+                            'latitude': _TPV['lat'],
+                            'longitude': _TPV['lon'],
+                            'altitude': _TPV['alt'],
+                            'speed': _speed,
+                            'valid': True
+                        }
+
+                        self.send_to_callback(_gps_state)
+
+
+            # Close the GPSD connection.
+            try:
+                _gpsd_socket.close()
+            except Exception as e:
+                logging.error("GPSD - Error when closing connection: %s" % str(e))
+
+
+
+
+
+if __name__ == '__main__':
+
+    def print_dict(data):
+        print(data)
+
+    logging.basicConfig(format='%(asctime)s %(levelname)s:%(message)s', level=logging.DEBUG)
+
+    _gpsd = GPSDAdaptor(callback=print_dict)
+    time.sleep(30)
+    _gpsd.close()
+
+
+    # gpsd_socket = GPSDSocket()
+    # data_stream = DataStream()
+    # gpsd_socket.connect()
+    # gpsd_socket.watch()
+    # for new_data in gpsd_socket:
+    #     if new_data:
+    #         data_stream.unpack(new_data)
+    #         print(data_stream.TPV)
+    #         #print(data_stream.SKY)

horusmapper.cfg.example

@@ -29,7 +29,7 @@ telemetry_source_port = 8942
 # none - No Chase-Car GPS
 # horus_udp - Read Horus UDP Broadcast 'Car GPS' messages
 # serial - Read GPS positions from a serial-connected GPS receiver.
-# gpsd - Poll GPSD for positions (TO BE IMPLEMENTED)
+# gpsd - Poll GPSD for positions
 car_source_type = horus_udp
 # Car position source port (UDP) - only used if horus_udp is selected
 car_source_port = 55672
@@ -48,7 +48,6 @@ car_source_port = 55672
 
 [gpsd]
 # GPSD Host/Port - Only used if selected in a telemetry profile above.
-# TO BE IMPLEMENTED
 gpsd_host = localhost
 gpsd_port = 2947
 

horusmapper.py

@@ -20,7 +20,8 @@ from dateutil.parser import parse
 from chasemapper.config import *
 from chasemapper.earthmaths import *
 from chasemapper.geometry import *
-from chasemapper.gps import SerialGPS, GPSDGPS
+from chasemapper.gps import SerialGPS
+from chasemapper.gpsd import GPSDAdaptor
 from chasemapper.atmosphere import time_to_landing
 from chasemapper.listeners import OziListener, UDPListener
 from chasemapper.predictor import predictor_spawn_download, model_download_running
@@ -663,8 +664,12 @@ def start_listeners(profile):
             data_listeners.append(_car_horus_udp_listener)
 
         elif profile['car_source_type'] == "gpsd":
-            # GPSD Car Position Source - TODO
+            # GPSD Car Position Source 
             logging.info("Starting GPSD Car Position Listener.")
+            _gpsd_gps = GPSDAdaptor(hostname=chasemapper_config['car_gpsd_host'],
+                port=chasemapper_config['car_gpsd_port'],
+                callback=udp_listener_car_callback)
+            data_listeners.append(_gpsd_gps)
 
         elif profile['car_source_type'] == "serial":
             # Serial GPS Source.