gpsstatus

#!/usr/bin/python -uO
# coding: UTF-8

from collections import defaultdict
from threading import Thread
import argparse
import curses
import fcntl
import locale
import math
import os
import re
import select
import signal
import struct
import sys
import termios
import time
import traceback
import tty
import serial
from serial import threaded
import curses.panel as panel

PREAMBLE = 0x2404
EPO_CMD = 0x02d2
EOW = 0x0a0d
UART_CMD = 253

SECONDS_PER_HOUR = 3600
GPS_OFFSET_SECONDS = 315964786
SECONDS_PER_WEEK = 604800
HOURS_PER_WEEK = 168

#locale.setlocale(locale.LC_ALL, 'en_US.utf8')
#code = locale.getpreferredencoding()
degsym = u'\N{DEGREE SIGN}'

#print(u'abc%s %s' % ("abc", degsym))
#sys.exit(0)

ex_string = None
width = 80
gps_device = None
hasfix = False
lostfix = time.time()
gotfix = time.time()
paused = False
nmea_paused = False
stdscr = None
ll = re.compile(r"(\d?\d\d)(\d\d)(\.\d\d\d\d)")
fix = defaultdict(lambda: "None", {"0": "None", "1": "GPS", "2": "DGPS", "6":"DRM"})
speed_units = defaultdict(lambda: "??", {"N": "Kn", "K": "KPH"})
modes_t = defaultdict(lambda: "NA", {"1": "NA", "2": "2D", "3": "3D"})
draws = {}
w = {}
p = {}
s = {}
f = {}

baudrate = {
    termios.B4800: 4800,
    termios.B9600: 9600,
    termios.B19200: 19200,
    termios.B38400: 38400,
    termios.B57600: 57600,
    termios.B115200: 115200,
}

baudrates = [4800, 9600, 19200, 38400, 57600, 115200 ]

def Convert2UTC(GPSHour):
    GPSHour *= SECONDS_PER_HOUR
    GPSHour += GPS_OFFSET_SECONDS
    return time.strftime("%Y-%m-%d %H:%M:%SZ", time.gmtime(GPSHour))

def Convert2Local(GPSHour):
    GPSHour *= SECONDS_PER_HOUR
    GPSHour += GPS_OFFSET_SECONDS
    return time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(GPSHour))

def crc8(d):
    crc = 0
    if isinstance(d, (bytearray)):
        ba = d
    else:
        ba = bytearray(d.encode("ASCII"))

    for b in ba:
        crc ^= (b & 0xff)
    return crc

def convert_arg_line_to_args(arg_line):
    for arg in arg_line.split():
        if not arg.strip():
            continue
        yield arg

def send_string(fg, string, flush=True):
    if isinstance(string, (bytearray)):
        ba = string
    elif isinstance(string, (bytes)):
        ba = bytearray(string)
    else:
        ba = bytearray(string.encode("ASCII"))

    buf = b"$%s*%02x\r\n" % (ba, crc8(ba))
    fg.write(buf)
    if flush:
        fg.flush()

def send_and_wait(fg, string, timeout, r="PMTK001,0,3", retries=0, debug=False):
    spl = None
    count = 0
    temp_timeout = fg.timeout
    fg.timeout = timeout
    while spl == None and count <= retries:
        resp = None
        if debug:
            print(">> %s" % string)
        send_string(fg, string, False);
        fg.reset_input_buffer()
        fg.flush()
        resp = fg.read_until()
        try:
            sresp = resp.decode("ASCII")
            if debug:
                print("<< %s" % sresp)
            ix = sresp.find(r)
            if ix >= 0:
                spl = sresp.strip()[ix:-3].split(',')
                break
        except:
            pass
        count = count + 1
        time.sleep(0.500)


    fg.timeout = temp_timeout
    return spl


def ping_unit(fg):
    resp = send_and_wait(fg, "PMTK000", 0.250, retries=1)
    if resp:
        return True
    return False

def set_nmea(fg, rate, quiet=False):
    if not quiet: print("Setting NMEA at %d baud." % rate)
    current_rate = fg.baudrate
    fg.baudrate = rate
    if ping_unit(fg):
        print("   Unit is currently in NMEA mode at %d baud." % rate);
        return True
    if not quiet: print("   No response.  Seeing if it's in binary mode.")
    buf = bytearray(b'\0' * 14)
    struct.pack_into("<HHHBIBH", buf, 0, PREAMBLE, 14, UART_CMD, 0, rate, 0, EOW)
    struct.pack_into("B", buf, 11, crc8(buf[2:11]))
    fg.flushInput()
    fg.write(buf)
    fg.flushOutput()
    time.sleep(0.500)
    if ping_unit(fg):
        if not quiet: print("   It might have been.  Anyway, it's now in NMEA mode at %d baud." % rate);
        return True
    if ping_unit(fg):
        if not quiet: print("   It might have been.  Anyway, it's now in NMEA mode at %d baud." % rate);
        return True
    if not quiet: print("   Apparently not. Failed.");
    fg.baudrate = current_rate
    return False

def set_speed(fg, current_rate, requested_rate, count=1, delay=0.250):
    speed_string = "PMTK251,%d" % requested_rate
    i = iter(range(count, -1, -1))
    while next(i):
        fg.baudrate = current_rate
        time.sleep(delay)
        fg.reset_input_buffer()
        send_string(fg, speed_string, True)
        resp = fg.read_until()
        fg.baudrate = requested_rate
        time.sleep(delay)
        if ping_unit(fg):
            return True
    fg.baudrate = current_rate
    return False

def cleanup(fi, fg, device, port_baudrate, keep):
    if fg:
        tty.setraw(fg, tty.TCSANOW)

    if fg and not keep:
        set_speed(fg, fg.baudrate, port_baudrate, 3, 0.057)
        fg.baudrate = port_baudrate

    if fg:
        fg.close()
    if fi:
        fi.close()

def get_known_state(device, speed, port_baudrate, keep):
    fg = serial.Serial(port=device, timeout=1)
#    tty.setraw(fg, tty.TCSANOW)

    if set_nmea(fg, speed):
        print("GPS and port are now synchronized at %d" % speed)
        return fg

    print("GPS isn't at the desired %d baud rate.  Trying the current port rate of %d baud." % (speed, port_baudrate))
    if set_nmea(fg, port_baudrate):
        print("Attempting to set it to %d baud" % speed)
        if set_speed(fg, port_baudrate, speed, 3, 0.250):
            print("GPS and port are now synchronized at %d" % speed)
            return fg
        else:
            print("Couldn't set the new speed for some reason.")
            return None

    print("GPS isn't at the current port rate either.")
    foundrate = 0
    for rate in baudrates:
        isnmea = set_nmea(fg, rate)
        if isnmea:
            foundrate = rate
            break

    if foundrate == 0:
        print("Unable to locate unit at port %s" % device)
        return None

    print("Found unit at %d baud.  Attempting to set it to %d baud" % (foundrate, speed))

    if not set_speed(fg, foundrate, speed, 1, 0.100):
        print("Failed to set baudrate to %d" % speed)
        return None

    print("GPS and port are now synchronized at %d" % speed)
    return fg

def format_dms(dddmm, direction):
    s = ll.split(dddmm)
    if len(s) < 3:
        return " " * 17
    return u'%3s%s %2s\' %05.2f\" %s' % (s[1], degsym if sys.version_info[0] >= 3 else "", s[2], float(s[3]) * 60.0, direction)

def draw_string(win, row, col, st, erase=-1):
    if erase < 0: erase = len(st)
    dw = win.derwin(1, erase, row, col)
    t1 = dw.instr(0, 0, erase)
    if (st == t1): return False
    dw.erase()
    try:
        dw.addstr(0, 0, st.encode('utf-8'))
    except:
        pass
#    print >>sys.stderr, time.time(), "'%s'" % t1, "'%s'" % st
    dw.refresh()
    del dw
    return True

def draw_win(wid, name, rows, cols, row, col):
    if wid in w: del w[wid]
    if wid in s: del s[wid]
    w[wid] = curses.newwin(int(rows), int(cols), int(row), int(col))
    p[wid] = panel.new_panel(w[wid])
    w[wid].border()
    w[wid].addstr(0, 2, name)
    w[wid].refresh()
    y, x = w[wid].getmaxyx()
    s[wid] = w[wid].derwin(y - 2, x - 2, 1, 1)
    if wid in draws: draws[wid]()

def draw_help():
#                                           1         2         3         4         5
#                                  123456789012345678901234567890123456789012345678901234567890
    draw_string(s['HELP'], 0, 0, "GPSSTATUS: quit, pause, pause nmea, refresh, version")
    draw_string(s['HELP'], 1, 0, "           cold start,  warm start, hot start")

    a = curses.A_BOLD | curses.A_UNDERLINE
    s['HELP'].chgat(0, 11, 1, a)
    s['HELP'].chgat(0, 17, 1, a)
    s['HELP'].chgat(0, 24, 1, a)
    s['HELP'].chgat(0, 36, 1, a)
    s['HELP'].chgat(0, 45, 1, a)
    s['HELP'].chgat(1, 11, 1, a)
    s['HELP'].chgat(1, 24, 1, a)
    s['HELP'].chgat(1, 36, 1, a)
    s['HELP'].refresh()
draws['HELP'] = draw_help


def draw_nav():
    win = s['NAV']
    draw_string(win, 0, 0, " Latitude:")
    draw_string(win, 1, 0, "Longitude:")
    draw_string(win, 0, 29, "  Alt(M):")
    draw_string(win, 1, 29, "  Alt(F):")

    draw_string(win, 0, 46, "Speed(KPH):")
    draw_string(win, 1, 46, "Speed(MPH):")

    draw_string(win, 0, 65, "Track:")
draws['NAV'] = draw_nav

def draw_time():
    win = s['TIME']
    draw_string(win, 0, 0, "Date:")
    draw_string(win, 0, 18, "Time:")
    draw_string(win, 0, 33, "UTC")
draws['TIME'] = draw_time

def draw_sat():
    win = s['SAT']
    draw_string(win, 0, 0, "  Sat:")
    draw_string(win, 1, 0, u"Elev°:" if sys.version_info[0] >= 3 else " Elev:")
    draw_string(win, 2, 0, u"  Az°:" if sys.version_info[0] >= 3 else "   Az:")
    draw_string(win, 3, 0, "  SNR:")

    draw_string(win, 5, 40, "Fix Type:")
    draw_string(win, 6, 40, "Num Sats:")
    draw_string(win, 5, 62, " TTFF(S):")
    draw_string(win, 6, 62, "Error(M):")

    x = win.getmaxyx()[1]
    win.hline(4, 0, curses.ACS_HLINE, x)
    win.vline(5, 39, curses.ACS_VLINE, 2)
    win.refresh()
    draw_string(win, 5, 0, "  EPO: ")
draws['SAT'] = draw_sat

def update_loc(lat, latdir, lon, londir):
    win = s['NAV']
    draw_string(win, 0, 11, format_dms(lat, latdir))
    draw_string(win, 1, 11, format_dms(lon, londir))
    return

def update_alt(alt):
    win = s['NAV']
    try:
        altM = float(alt)
        altF = altM * 3.28084
        draw_string(win, 0, 39, "%5.0f" % altM)
        draw_string(win, 1, 39, "%5.0f" % altF)
    except:
        draw_string(win, 0, 39, " " * 5)
        draw_string(win, 1, 39, " " * 5)
    return

def update_time(date, tm):
    win = s['TIME']
    draw_string(win, 0, 6, date)
    draw_string(win, 0, 24, tm)
    return

def init_windows():
    global width, gps_device

    s['HELP'] = curses.newwin(2, width, 0, 0)
    draw_help()

    draw_win('TIME', "Date/Time", 3, width / 2, 2, 0)
    draw_win('STATUS', "Status", 3, width / 2, 2, width / 2)
    draw_string(s['STATUS'], 0, 0, "Running")
    draw_string(s['STATUS'], 0, 15, "Device: %s" % gps_device)
    draw_win('NAV', "Navigation", 4, width, 5, 0)
    draw_win('SAT', "Satellites", 9, width, 9, 0)
    draw_win('NMEA', "NMEA", 12, width, 18, 0)
    s['NMEA'].scrollok(True)

def GGA(a):
    global lostfix, hasfix, gotfix
    if len(a) < 11:
        return

    update_loc(a[2], a[3], a[4], a[5])
    update_alt(a[9])

    newhasfix = (fix[a[6]] != "None")

    if hasfix and not newhasfix:
        lostfix = time.time()

    if not hasfix and newhasfix:
        gotfix = time.time()

    hasfix = newhasfix

    if hasfix:
        ttff = gotfix - lostfix
    else:
        ttff = time.time() - lostfix

    win = s['SAT']
    draw_string(win, 5, 50, "%4.4s" % fix[a[6]])
    draw_string(win, 6, 50, "%4d" % int(a[7]))
    draw_string(win, 5, 72, "%5d" % ttff)

    corr = 1.5 if fix[a[6]] == "DGPS" else 5.1
    try:
        error = float(a[8]) * corr
    except:
        error = -0.0
    draw_string(win, 6, 72, "%5.1f" % error)
f['GGA'] = GGA

def GLL(a):
    if len(a) < 5:
        return
    update_loc(a[1], a[2], a[3], a[4])
f['GLL'] = GLL

def RMC(a):
    if len(a) < 10:
        return
    if len(a[9]):
        update_time("20%2s-%2s-%2s" % (a[9][4:6], a[9][2:4], a[9][0:2]),
            "%2s:%2s:%2s" % (a[1][0:2], a[1][2:4], a[1][4:6]))
    update_loc(a[3], a[4], a[5], a[6])

    try:
        VTG(["", a[8], "T", "", "", a[7], "", float(a[7]) * 1.852, a[8]])
    except:
        pass
f['RMC'] = RMC

def GSA(a):
    if len(a) < 3:
        return
    draw_string(s['SAT'], 5, 55, "%3.3s" % modes_t[a[2]])
f['GSA'] = GSA

def GSV(a):
    w = s['SAT']

#    messages = a[1].isdigit() and int(a[1]) or 0
    message = a[2].isdigit() and int(a[2]) or 0
#    siv = a[3].isdigit() and int(a[3]) or 0

    s1 = None
    s2 = None
    s3 = None
    s4 = None

    if len(a) >= 8:
        s1 = a[4:8]

    if len(a) >= 12:
        s2 = a[8:12]

    if len(a) >= 16:
        s3 = a[12:16]

    if len(a) >= 20:
        s4 = a[16:20]

    col = 7 + ((message - 1) * 16)

    for i in range(0, 4):
        t2 = " %3s %3s %3s %3s" % (
        s1[i] if s1 else " ",
        s2[i] if s2 else " ",
        s3[i] if s3 else " ",
        s4[i] if s4 else " ")
        draw_string(w, i, col, t2)

f['GSV'] = GSV

def VTG(a):
    if len(a) < 9:
        return
    w = s['NAV']

    draw_string(w, 0, 72, "%3.0f" % float(a[1]))
    draw_string(w, 0, 75, "\xb0")
    draw_string(w, 0, 76, a[2])

    try:
        s1 = float(a[5]) * 1.15078
        s2 = float(a[7])
    except:
        s1 = 0.0
        s2 = 0.0

    draw_string(w, 0, 58, "%5.1f" % s2)
    draw_string(w, 1, 58, "%5.1f" % s1)

f['VTG'] = VTG

def ZDA(a):
    w = s['TIME']
    if len(a) < 4:
        return
    update_time("%4s-%2s-%2s" % (a[4], a[3], a[2]),
        "%2s:%2s:%2s" % (a[1][0:2], a[1][2:4], a[1][4:6]))
f['ZDA'] = ZDA


def TK707(a):
    w = s['SAT']
    vstart = int(int(a[2]) * 168 + (int(a[3]) / 3600))
    vend = int(int(a[4]) * 168 + (int(a[5]) / 3600))
    draw_string(w, 5, 10, "From: %s" % (Convert2UTC(vstart)))
    draw_string(w, 6, 10, "  To: %s" % (Convert2UTC(vend)))
f['TK707'] = TK707

def TK705(a):
    draw_string(s['STATUS'], 0, 0, ','.join(str(x) for x in a[1:]))
f['TK705'] = TK705

def getheightwidth():
    try:
        return int(os.environ["LINES"]), int(os.environ["COLUMNS"])
    except KeyError:
        height, width = struct.unpack(
            "hhhh", fcntl.ioctl(0, termios.TIOCGWINSZ , "\000"*8))[0:2]
        if not height: return 25, 80
        return height, width



class SerialReceiver(serial.threaded.LineReader):

    TERMINATOR = b'\r\n'

    def __init__(self):
        super(SerialReceiver, self).__init__()
        self.last_sent = int(time.time())
        self.last_msg = 0
        self.nline = 0

    def handle_line(self, line):
        line = line.strip()
        if len(line) == 0 or line[0] != "$":
            return;
        line = line[1:-3]
        a = line.split(',')
        stype = a[0][2:]
        try:
            if stype in f:
                f[stype](a)

            curr_msg = int(time.time())
            if curr_msg != self.last_msg:
                self.last_msg = curr_msg
                s['NMEA'].erase()
                self.nline = 0

            if not nmea_paused:
                y, x = s['NMEA'].getmaxyx()
                s['NMEA'].addnstr(min(self.nline, y - 1), 0, line, x)
                s['NMEA'].refresh()
                self.nline += 1
        except:
            ex_string = traceback.format_exc()
            curses.flushinp()
            curses.ungetch('q')

        tm = int(time.time())
        if tm % 10 == 0 and self.last_sent != tm:
    #        send_string(fg, "PMTK607")
            self.last_sent = tm

def input_loop(fg):
    global paused, nmea_paused, stdscr
    while True:
        qq = stdscr.getch()
        if qq <= 0:
            time.sleep(0.250)
            continue
        qq = chr(qq)
        if qq == 'q':
            draw_string(s['STATUS'], 0, 0, "Terminating", 15)
            break
        elif qq == 'p':
            paused = not paused
            if paused:
                draw_string(s['STATUS'], 0, 0, "Paused", 15)
            else:
                draw_string(s['STATUS'], 0, 0, "Running", 15)
        elif qq == 'n':
            nmea_paused = not nmea_paused
            if nmea_paused:
                draw_string(w['NMEA'], 0, 7, "Paused")
            else:
                w['NMEA'].hline(0, 7, curses.ACS_HLINE, 6)
                w['NMEA'].refresh()

        elif qq == 'r':
            paused = True
            refresh_windows()
            paused = False
        elif qq == 'h':
            send_string(fg, "PMTK101")
            draw_string(s['STATUS'], 0, 0, "Hot Started")
        elif qq == 'w':
            send_string(fg, "PMTK102")
            draw_string(s['STATUS'], 0, 0, "Warm Started")
        elif qq == 'c':
            send_string(fg, "PMTK103")
            draw_string(s['STATUS'], 0, 0, "Cold Started")
        elif qq == 'v':
            send_string(fg, "PMTK605")
        elif qq == 'e':
            send_string(fg, "PMTK607")
        elif qq == 'q':
            break

class input_loop_thread(Thread):
    def __init__(self, fg):
        Thread.__init__(self)
        self.fg = fg

    def run(self):
        input_loop(self.fg)

def refresh_windows():
    for sid in s:
        s[sid].erase()
        s[sid].refresh()
        if sid in draws: draws[sid]()
    draw_string(s['STATUS'], 0, 0, "Running")
    draw_string(s['STATUS'], 0, 15, "Device: %s" % gps_device)
    send_string(fg, "PMTK607")

def sigwinch_handler(n, frame):
    global width, paused
    paused = True
    y, x = getheightwidth()
    curses.resizeterm(y, x)
    init_windows()
    paused = False

class gpsproto(SerialReceiver):
    def __init__(self):
        super(gpsproto, self).__init__()

def main(stds, _fg):
    global paused, nmea_paused, stdscr, width, gps_device
    global lostfix, hasfix, gotfix
    global fg

    signal.signal(signal.SIGWINCH, sigwinch_handler)
    fg = _fg
    gps_device = fg.port
    lostfix = time.time()
    hasfix = False
    gotfix = time.time()

    stdscr = stds
    stdscr.refresh()
    curses.curs_set(0)

    init_windows()

    with serial.threaded.ReaderThread(fg, gpsproto) as protocol:
        send_string(fg, "PMTK607")
        input_loop(fg)

    if fg and fg.isOpen:
        fg.close()

def startup():
    parser = argparse.ArgumentParser(fromfile_prefix_chars='@',
    description="Displays NMEA Data",
    epilog="You can use '@filename' to read arguments from a file.")
    parser.convert_arg_line_to_args = convert_arg_line_to_args
    parser.add_argument("-s", "--speed", type=int, default=-1, dest="speed", help="Interface speed", choices=[4800, 9600, 19200, 38400, 57600, 115200])
    parser.add_argument("-n", "--no-init", dest="no_init", default=False, action="store_true", help="Skip the initialization, just run the commands")
    parser.add_argument("-k", "--keep-new-speed", dest="keep_new_speed", default=True, action="store_true", help="Don't restore the old speed on exit")
    parser.add_argument("gps_device", metavar="<GPS_Device>", help="GPS Device")
    args = parser.parse_args()

    tfg = None
    try:
        tfg = os.open(args.gps_device, os.O_RDWR)
        params = termios.tcgetattr(tfg);
        port_baudrate = baudrate[params[5]];
        tty.setraw(tfg, tty.TCSANOW)
    except:
        print(sys.exc_info()[1])
        return None
    finally:
        if tfg is not None: os.close(tfg)

    if args.speed < 0:
        args.speed = port_baudrate

    if args.no_init:
        fg = serial.Serial(args.gps_device, timeout=5, baudrate=args.speed)
    else:
        fg = get_known_state(args.gps_device, args.speed, port_baudrate, args.keep_new_speed)

    if fg is None:
        print("Could not open port at %s" % args.gps_device)
        return None

    return fg

if __name__ == "__main__":
    fg = startup()
    if fg is None:
        sys.exit(1)
    rc = curses.wrapper(main, fg)
    if ex_string: print(ex_string)
    sys.exit(rc)