epoloader

#!/usr/bin/python -uO

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
SATS_PER_EPO = 32
SECONDS_PER_HOUR = 3600
GPS_OFFSET_SECONDS = 315964786
SECONDS_PER_WEEK = 604800
HOURS_PER_WEEK = 168

baudrate = {
    termios.B4800: 4800,
    termios.B9600: 9600,
    termios.B19200: 19200,
    termios.B38400: 38400,
    termios.B57600: 57600,
    termios.B115200: 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 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()
        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 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 cleanup(fi, fg, device, previous_baudrate, keep):
    if fg:
        tty.setraw(fg, tty.TCSANOW)

    if fg and not keep:
        print "Resetting NMEA Speed: %d" % previous_baudrate
        send_speed(fg, previous_baudrate, 3, 0.057)
        fg.baudrate = previous_baudrate

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

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="Loads EPO data sets to MT3339 GPS",
    epilog="You can use '@filename' to read arguments from a file.")
    parser.convert_arg_line_to_args = convert_arg_line_to_args
    group = parser.add_argument_group(title="optional known time and location parameters")
    group.add_argument("-t", "--time", dest="time_string", help="Current UTC or UTC from host", metavar="yyyy,mm,dd,hh,mm,ss | - ")
    group.add_argument("-l", "--location", dest="location_string", help="Current location specified in decimal degrees and meters", metavar="lat.dddddd,lon.dddddd,alt")

    parser.add_argument("-s", "--speed", type=int, default=115200, dest="speed", help="Interface speed", choices=[4800, 9600, 19200, 38400, 57600, 115200])
    parser.add_argument("-k", "--keep-new-speed", dest="keep_new_speed", default=False, action="store_true", help="Don't restore the old speed on exit")
    parser.add_argument("-c", "--clear", dest="clear_epo", default=False, action="store_true", help="Clears the existing EPO data from the unit")
    parser.add_argument("-n", "--no-init", dest="no_init", default=False, action="store_true", help="Skips the initialization")

    parser.add_argument("input_file", metavar="<EPO_File>", help="EPO File or '-' to just set the known parameters")
    parser.add_argument("output_device", metavar="<gps_device>", help="GPS serial device such as '/dev/ttyUSB0'")

    args = parser.parse_args()

    fi = None
    if args.input_file != "-":
        try:
            file_size = os.stat(args.input_file).st_size
            fi = io.open(args.input_file, mode="rb")
        except:
            print sys.exc_info()[1]
            return 1

        header = fi.read(75)
        fi.close()
        if header[0:3] == header[60:63]:
            print "Opening EPO Type I file"
            EPO_SET_SIZE = 1920
            SAT_SET_SIZE = 60
            FRAME_LENGTH = 191
        elif header[0:3] == header[72:75]:
            print "Opening EPO Type II file"
            EPO_SET_SIZE = 2304
            SAT_SET_SIZE = 72
            FRAME_LENGTH = 227
        else:
            print >> sys.stderr, "%s is not a valid EPO file." % args.input_file
            return 1

        if file_size % EPO_SET_SIZE != 0:
            print >> sys.stderr, "The size of %s is not an even multiple of EPO_SET_SIZE(%d).  It may be corrupted." % (args.input_file, EPO_SET_SIZE)
            return 1

        fi = io.open(args.input_file, mode="rb", buffering=0)

    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;

    try:
        tfg = os.open(args.output_device, os.O_RDWR)
    except:
        print sys.exc_info()[1]
        return 1

    params = termios.tcgetattr(tfg);
    previous_baudrate = baudrate[params[5]];
    tty.setraw(tfg, tty.TCSANOW)
    os.close(tfg)

    print "Current port speed: %d" % previous_baudrate

    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, previous_baudrate, args.keep_new_speed)

    if fg is None:
        return 1

    time.sleep(0.500)
    if not args.location_string is None or not args.time_string is None:
        if args.time_string == "-":
            t = time.time();
            tm = time.gmtime(t)
            time_string = time.strftime("%Y,%m,%d,%H,%M,%S", tm)
        else:
            time_string = args.time_string

        print "Setting known values: %s  %s" % (args.location_string, time_string)
        resp = send_and_wait(fg, "PMTK740,%s" % time_string, 2, retries=1)
        if resp is None or resp[2] != '3':
            print >> sys.stderr, "ERROR: Unable to set time."
            cleanup(fi, fg, args.output_device, previous_baudrate, args.keep_new_speed)
            return 1

        resp = send_and_wait(fg, "PMTK741,%s,%s" % (args.location_string, time_string), 2, retries=1)
        if resp is None or resp[2] != '3':
            print >> sys.stderr, "ERROR: Unable to set location."
            cleanup(fi, fg, args.output_device, previous_baudrate, args.keep_new_speed)
            return 1

    if args.clear_epo:
        print "Clearing existing EPO data"
        resp = send_and_wait(fg, "PMTK127", 2, retries=1)
        if resp is None or resp[2] != '3':
            print >> sys.stderr, "ERROR: Unable to clear existing EPO data."
            cleanup(fi, fg, args.output_device, previous_baudrate, args.keep_new_speed)
            return 1

    if fi is None:
        cleanup(fi, fg, args.output_device, previous_baudrate, args.keep_new_speed)
        return 0

    print "Setting binary mode, speed: %d" % args.speed
    send_string(fg, "PMTK253,1,%d" % args.speed)
    time.sleep(0.500)

    seq = 0;
    total_read = 0;
    failed = False
    epo_sets = 0

    print "Sending %d EPO sets" % (file_size / EPO_SET_SIZE)
    while total_read < file_size:
        for lseq in range(11):
            buf = bytearray('\0' * FRAME_LENGTH)
            struct.pack_into("<HHHH", buf, 0, PREAMBLE, FRAME_LENGTH, EPO_CMD, seq)
            data_start = 8
            if lseq == 10:
                data_end = data_start + SAT_SET_SIZE * 2
            else:
                data_end = data_start + SAT_SET_SIZE * 3

            red = fi.read(data_end - data_start)
            total_read += len(red)
            buf[data_start:data_end] = red

            struct.pack_into("<BH", buf, FRAME_LENGTH - 3, crc8(buf[2: FRAME_LENGTH - 3]), EOW)

            if lseq == 0:
                epo_sets += 1
                set_start_time = struct.unpack("<I", red[0:3] + '\0')[0]
                print "Sending set %4d.  Valid from %s UTC" % (epo_sets, Convert2UTC(set_start_time))
                if seq == 0:
                    start = set_start_time

            fg.write(buf)
            fg.flush();
            rseq, result = getResponse(fg, 2)
            if result != 1:
                failed = True
                print >> sys.stderr, "Transfer failed"
                break
            if seq != rseq:
                failed = True
                print >> sys.stderr, "Sequence error: %d != %d" % (seq, result)
                break
            seq += 1

        if failed:
            break

    fi.close()
    if not failed:
        end = set_start_time
        buf = bytearray('\0' * FRAME_LENGTH)
        struct.pack_into("<HHHH", buf, 0, PREAMBLE, FRAME_LENGTH, EPO_CMD, 0xffff)
        struct.pack_into("<BH", buf, FRAME_LENGTH - 3, crc8(buf[2: FRAME_LENGTH - 3]), EOW)
        fg.write(buf)
        fg.flush()
        seq, result = getResponse(fg, 2)
        if result != 1:
            print >> sys.stderr, "Final Transfer failed"
        else:
            print "%4d sets sent.  Valid from %s to %s UTC" % (epo_sets,
            Convert2UTC(start),
            Convert2UTC(end + 6))

    set_nmea(fg, args.speed)
    getResponse(fg, 1)
    time.sleep(1.00)

    resp = send_and_wait(fg, "PMTK607", 2, "PMTK707", retries=1)
    cleanup(fi, fg, args.output_device, previous_baudrate, args.keep_new_speed)

    if resp is None:
        print >> sys.stderr, "ERROR: EPO in NVRAM couldn't be verified"
        return 1

    vstart = (int(resp[2]) * 168 + int(resp[3]) / 3600)
    vend = (int(resp[4]) * 168 + int(resp[5]) / 3600)
    if vstart == start and vend == end:
        print "Verified EPO in NVRAM matches file"
        return 0
    else:
        print >> sys.stderr, "ERROR: EPO in NVRAM doesnt match file"
        print >> sys.stderr, "  In NVRAM: %s to %s UTC" % (Convert2UTC(vstart), Convert2UTC(vend))
        return 1

if __name__ == "__main__":

    sys.exit(main() or 0)