gpsinit

#!/usr/bin/python


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

import serial


PREAMBLE = 0x2404
EPO_CMD = 0x02d2
EOW = 0x0a0d
UART_CMD = 253
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,
}

def crc8(d):
    crc = 0
    for b in bytearray(d):
        crc ^= (b & 0xff)
    return crc

def getResponse(fg, command):
    limit = 0
    while True:
        b = fg.read(1)
        if len(b) == 0:
            break

        if b != '\x04':
            continue

        b = fg.read(1)
        if b != '\x24':
            continue

        length, cmd = struct.unpack("<HH", fg.read(4))

        if cmd == command:
            return struct.unpack("<HBBH", fg.read(6))[0:2]
        else:
            struct.unpack("<%dBBH" % (length - 9), fg.read(length - 6))

        limit += 1
        if limit > 10:
            break

    return (-1, -1, -1, -1)


class rx_loop_thread(Thread):
    def __init__(self, fg, r):
        Thread.__init__(self)
        self.fg = fg
        self.result = None;
        self.killme = False
        self.r = r

    def getResult(self):
        return self.result

    def kill(self):
        self.killme = True

    def run(self):
        self.fg.flushInput()
        while True:
            try:
                for line in self.fg:
                    if self.killme: return
                    if len(line) < 3:
                        continue
                    spl = line.strip()[1:-3].split(',')
                    if spl[0] == self.r:
                        self.result = spl
                        return
            except:
                pass

def send_and_wait(fg, string, timeout, r="PMTK001", retries=0):
    resp = None
    count = 0

    while resp == None and count <= retries:
        thread = rx_loop_thread(fg, r)
        thread.setDaemon(True)
        thread.start()
        fg.flushOutput()
        send_string(fg, string);
        thread.join(timeout)
        if thread.isAlive():
            thread.kill()
            count += 1
            continue
        return thread.getResult()
    return resp

def send_string(fg, string):
    fg.write("$%s*%02x\r\n" % (string, crc8(string)))
    fg.flush()

def send_speed(fg, speed, count, delay):
    speed_string = "PMTK251,%d" % speed
    send_string = "$%s*%02x\r\n" % (speed_string, crc8(speed_string))
    i = iter(range(count, -1, -1))
    while next(i):
        fg.write(send_string)
        fg.flush()
        time.sleep(delay)

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 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):
    buf = bytearray('\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.write(buf)
    fg.flush()

def set_speed(fg, speed):
    send_speed(fg, speed, 1, 0.100)
    fg.baudrate = speed

def process_command(fg, line):
    line = str(line)
    if line[0] == '-':
        send_string(fg, line[1:])
        time.sleep(0.250)
    else:
        resp = send_and_wait(fg, line, 5)
        if resp is None or resp[2] != '3':
            print >> sys.stderr, "Failed to get ACK."

def process_line(fg, device, speed, line):
    rc = 0
    if line[0:len("setspeed ")] == "setspeed ":
        newspeed = int(line[len("setspeed "):].strip())
        print "Setting new speed: %d" % newspeed
        set_speed(fg, 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("hot_start") >= 1:
        process_command(fg, line.replace("hot_start", "PMTK101"))
    elif line.count("warm_start") >= 1:
        process_command(fg, line.replace("warm_start", "PMTK102"))
    elif line.count("cold_start") >= 1:
        process_command(fg, line.replace("cold_start", "PMTK103"))
    elif line == "factory_reset":
        process_command(fg, "-PMTK104")
        time.sleep(0.500)
        set_speed(fg, 9600)
    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()
            if not pline:
                break
            print ">> " + pline.strip()
    elif line[0:len("epoloader ")] == "epoloader ":
        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()
            if not pline:
                break
            print ">> " + pline.strip()
        fg = serial.Serial(port=device, timeout=5, baudrate=speed)
    else:
        process_command(fg, line)
    return (rc, fg)

def get_known_state(device, speed, previous_baudrate, keep):
    fg = serial.Serial(port=device, timeout=5, baudrate=speed)
    tty.setraw(fg, tty.TCSANOW)
    fg.flushInput()
    resp = send_and_wait(fg, "PMTK000", 0.5, retries=1)
    if resp and resp[2] == '3':
        print "GPS and port are now synchronized at %d" % speed
        return fg

#    Make sure the unit is in NMEA mode
    print "Making sure the unit is in NMEA mode"
    for t in baudrate:
        rate = baudrate[t]
        fg.baudrate = rate
        set_nmea(fg, rate)
        set_nmea(fg, rate)
    fg.flushInput()
    fg.flushOutput()
    time.sleep(0.500)
    tries = 0
    while True:
        print "Making sure the speeds match"

        for t in baudrate:
            fg.baudrate = baudrate[t]
            send_speed(fg, speed, 1, 0.100)
            fg.flush()

        fg.baudrate = speed
        time.sleep(0.250)
        fg.flushInput()
        # Now try to send a NoOp command
        resp = send_and_wait(fg, "PMTK000", 0.50, retries=1)
        if resp is None or resp[2] != '3':
            print >> sys.stderr, "The GPS speed and the port speed couldn't be synchronized."
            tries += 1
            if tries < 2:
                print "Retrying"
            else:
                return None
        else:
            print "GPS and port are now synchronized at %d" % speed
            return fg

def main():
    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=115200, 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("output_device", metavar="<gps_device>", help="GPS serial device such as '/dev/ttyUSB0'")

    args = parser.parse_args()

    p = subprocess.Popen("lsof -FpcL %s 2>/dev/null" % args.output_device,
        stdin=subprocess.PIPE, stdout=subprocess.PIPE, close_fds=True, shell=True)
    o = p.communicate()[0]
    if len(o) > 0:
        lines = o.split('\n')
        print >> sys.stderr, "ERROR: Device %s is open by PID: %s  PROG: %s  USER: %s" % (args.output_device, lines[0][1:], lines[1][1:], lines[2][1:])
        return 1;

    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, None, False)

    if fg is None: return 1

    rc = 0
    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}", args.output_device).replace("${SPEED}", str(args.speed))
            print line
            (rc, fg) = process_line(fg, args.output_device, args.speed, line)

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

    tty.setraw(fg, tty.TCSANOW)
    fg.close()
    return rc

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