gpsinit

#!/usr/bin/python


from os.path import dirname
import argparse
import io
import os
import struct
import subprocess
import sys
import termios
import time
import tty

import serial

global output_device
global keep_new_speed
global speed
global debug
global enabled_messages
global fg

PREAMBLE = 0x2404
EPO_CMD = 0x02d2
EOW = 0x0a0d
UART_CMD = 253
SATS_PER_EPO = 32
SECONDS_PER_HOUR = 3600
GPS_OFFSET_SECONDS = 315964786
SECONDS_PER_WEEK = 604800
HOURS_PER_WEEK = 168
DEFAULT_COMMAND = "PMTK314,1,1,1,1,5,5,0,0,0,0,0,0,0,0,0,0,0,1,0"

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:%S", 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
    for b in bytearray(d):
        crc ^= (b & 0xff)
    return crc

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))
    if debug:
        print(">> %s" % buf.decode("ASCII").strip())
    fg.write(buf)
    if flush:
        fg.flush()

def send_and_wait(fg, string, timeout, r="PMTK001,0,3", retries=0):
    spl = None
    count = 0
    temp_timeout = fg.timeout
    fg.timeout = timeout
    while spl == None and count <= retries:
        resp = None
        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.strip())
            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 convert_arg_line_to_args(arg_line):
    for arg in arg_line.split():
        if not arg.strip():
            continue
        yield arg

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 setclock(fg):
    uid = os.geteuid()
    if uid != 0:
        print("set_system_clock must be run as root")
        return -1

    print("Setting system clock")

    resp = send_and_wait(fg, "PMTK414", 2, r="PMTK514", retries=2)
    if not resp or len(resp) < 18:
        print("Failed to get current NMEA sentence info")
        return -1

    resp[18] = '1'
    resp = send_and_wait(fg, ','.join(resp), 3, retries=2)
    if not resp:
        print("Failed to set NMEA sentence info (may still work)")

    resp = send_and_wait(fg, "", 3, r="GPZDA", retries=2)
    if not resp:
        print("Failed to get date/time")
        return -1

    tt = (int(resp[4]), int(resp[3]), int(resp[2]), int(resp[1][0:2]), int(resp[1][2:4]), int(resp[1][4:6]), 0)
    tts = "%4d-%02d-%02d %02d:%02d:%02d" % tt[:6]

    if tt[0] < 2015:
        print("The GPS time %s is not valid." % tts)
        return -1

    rc = subprocess.call(['date', '+%Y-%m-%d %H-%M-%S', '-u', '-s',  '%s' % tts])
    if rc != 0:
        print("Failed to set date/time: RC %d.  Are you root?" % rc)
        return -1

    print("Set system time to %s UTC" % tts)

def process_command(fg, line, expected_response="PMTK"):
    rc = 0
    line = str(line)
    if line[0] == '-':
        send_string(fg, line[1:])
        time.sleep(0.250)
        return (0, None)
    else:
        resp = send_and_wait(fg, line, 5, r=expected_response)
        if resp is None:
            print("Failed to get ACK.")
            rc = 1
    return (rc, resp)

def process_line(line):
    global output_device
    global keep_new_speed
    global speed
    global debug
    global enabled_messages
    global fg

    rc = 0
    resp = None
    if line[0:len("setspeed ")] == "setspeed ":
        newspeed = int(line[len("setspeed "):].strip())
        print("Setting new speed: %d" % newspeed)
        port_baudrate = fg.baudrate
        fg.close()
        fg = get_known_state(output_device, newspeed, port_baudrate, keep_new_speed)
        if fg is None:
            rc = 1
        speed = newspeed
    elif line[0:len("sleep ")] == "sleep ":
        sleeptime = float(line[len("sleep "):].strip())
        print("Sleeping for %f seconds" % sleeptime)
        time.sleep(sleeptime)
    elif line.count("get_version") >= 1:
        (rc, resp) = process_command(fg, line.replace("get_version", "PMTK605"), expected_response="PMTK705")
        if rc == 0:
            print(resp, file=sys.stderr)
    elif line.count("get_epo") >= 1:
        (rc, resp) = process_command(fg, line.replace("get_epo", "PMTK607"), expected_response="PMTK707")
        if rc == 0:
            vstart = int(int(resp[2]) * 168 + int(resp[3]) / 3600)
            vend = int(int(resp[4]) * 168 + int(resp[5]) / 3600)
            print("  EPO in NVRAM: %s to %s UTC" % (Convert2UTC(vstart), Convert2UTC(vend)), file=sys.stderr)
    elif line.count("debug_on") >= 1:
        debug = True
    elif line.count("debug_off") >= 1:
        debug = False
    elif line.count("hot_start") >= 1:
        (rc, resp) = process_command(fg, line.replace("hot_start", "PMTK101"), expected_response="$PGACK")
    elif line.count("warm_start") >= 1:
        (rc, resp) = process_command(fg, line.replace("warm_start", "PMTK102"), expected_response="$PGACK")
    elif line.count("cold_start") >= 1:
        (rc, resp) = process_command(fg, line.replace("cold_start", "PMTK103"), expected_response="$PGACK")
    elif line.count("clear_epo") >= 1:
        (rc, resp) = process_command(fg, "PMTK127", expected_response="$CLR,EPO")
    elif line.count("save_enabled_messages") >= 1:
        (rc, resp) = process_command(fg, "PMTK414", expected_response="PMTK514")
        if rc == 0:
            enabled_messages = resp
    elif line.count("restore_enabled_messages") >= 1:
        if enabled_messages is not None:
            send_cmd = enabled_messages
            send_cmd[0] = "PMTK314"
            (rc, resp) = process_command(fg, ','.join(send_cmd), expected_response="PMTK001,314,3")
    elif line == "factory_reset":
        (rc, _resp) = process_command(fg, "-PMTK104")
        time.sleep(0.500)
        port_baudrate = fg.baudrate
        fg.close()
        fg = get_known_state(output_device, speed, port_baudrate, keep_new_speed)
        if fg is None:
            rc = 1
    elif line == "set_system_clock":
        rc = setclock(fg)
    elif line[0:len("eporetrieve ")] == "eporetrieve ":
        a0 = sys.argv[0]
        d = dirname(a0) + '/'
        p = subprocess.Popen(d + line,
            stdin=subprocess.PIPE, stdout=subprocess.PIPE, close_fds=True, shell=True)
        while True:
            pline = p.stdout.readline().decode("utf-8")
            if not pline:
                break
            print(">>> " + pline.strip())
    elif line[0:len("epoloader ")] == "epoloader ":
        port_baudrate = fg.baudrate
        fg.close()
        a0 = sys.argv[0]
        d = dirname(a0) + '/'
        p = subprocess.Popen(d + line,
            stdin=subprocess.PIPE, stdout=subprocess.PIPE, close_fds=True, shell=True)
        while True:
            pline = p.stdout.readline().decode("utf-8")
            if not pline:
                break
            print(">> " + pline.strip())
        fg = get_known_state(output_device, speed, port_baudrate, keep_new_speed)
        if fg is None:
            rc = 1
    else:
        (rc, resp) = process_command(fg, line)
        if resp is not None:
            print(rc,resp)
    return (rc, resp)


def main():
    global output_device
    global keep_new_speed
    global speed
    global debug
    global enabled_messages
    global fg

    parser = argparse.ArgumentParser(fromfile_prefix_chars='@',
    description="Initialized GPS to known state",
    epilog="You can use '@filename' to read arguments from a file.")
    group = parser.add_mutually_exclusive_group()
    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])
    group.add_argument("-i", "--init_command", metavar="<init_command>", dest="init_command", help="A single initialization command such as 'PMTK101'")
    group.add_argument("-f", "--command_file", metavar="<command_file>", dest="command_file", help="A file containing commands used to initialize the GPS")
    group.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("-d", "--debug", default=False, dest="debug", action="store_true", help="Print debugging info")

    parser.add_argument("output_device", metavar="<gps_device>", help="GPS serial device such as '/dev/ttyUSB0'")

    args = parser.parse_args()

    try:
        tfg = os.open(args.output_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 1
    finally:
        os.close(tfg)

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

    speed = args.speed
    debug = args.debug

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

    if fg is None: return 1

    rc = 0
    fi = None
    enabled_messages = None
    (rc, resp) = process_command(fg, "PMTK414", expected_response="PMTK514")
    if rc == 0:
        enabled_messages = resp

    if args.command_file:
        fi = io.open(args.command_file, mode="r")
        print("Processing commands...")
        while True:
            line = fi.readline()
            if not line or len(line) <= 0:
                break;
            line = line.strip()
            if len(line) == 0:
                continue
            if line[0] == "#":
                continue
            line = line.replace("${DEVICE}", output_device).replace("${SPEED}", str(speed))
            print(">> %s" % line)
            (rc, resp) = process_line(line)
            if rc != 0:
                print(rc);
                break

        fi.close()
        print("Done")
    elif args.init_command:
        (rc, resp) = process_line(args.init_command)
    else:
        (rc, resp) = process_line(DEFAULT_COMMAND)

    cleanup(fi, fg, args.output_device, port_baudrate, args.keep_new_speed)

    return rc

if __name__ == "__main__":
    sys.exit(main() or 0)