chasemapper/chasemapper/gpsd.py

#!/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
import traceback
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)