racerxdl / gps.py

#!/usr/bin/env python import serial import struct ser = serial.Serial("/dev/ttyUSB0", 115200, timeout=2) msgid = { 0x10 : "GPS", 0x20 : "MAG" } msgid_inv = { "GPS" : 0x10, "MAG" : 0x20 } fixtype = { 0x00 : "No Fix", 0x02 : "2D Lock", 0x03 : "3D Lock" } def ParseDateTime(data): seconds = (data & 0x3F ) >> 0 minutes = (data & 0xFC0 ) >> 6 hours = (data & 0xF000 ) >> 12 day = (data & 0x1F0000 ) >> 16 month = (data & 0x1E00000 ) >> 21 year = (data & 0xFE000000 ) >> 25 return {"seconds":seconds,"minutes":minutes,"hours":hours,"day":day,"month":month,"year":year} def bin(s): return str(s) if s<=1 else bin(s>>1) + str(s&1) def GenMagMask(b9): b9b = bin(b9).rjust(8,"0") _0 = int(b9b[7]) _1 = int(b9b[6]) _2 = int(b9b[5]) _3 = int(b9b[4]) _4 = int(b9b[3]) _5 = int(b9b[2]) _6 = int(b9b[1]) _7 = int(b9b[0]) mask = [0,0,0,0,0,0,0,0] mask[7] = _0 ^ _4 mask[6] = _1 ^ _5 mask[5] = _2 ^ _6 mask[4] = _3 ^ _7 ^ _0 mask[3] = _1 mask[2] = _2 mask[1] = _3 mask[0] = _4 ^ _0 mask = int("".join([str(x) for x in mask]), 2) return mask while True: buff = ser.read(2) if buff == "\x55\xAA": #print "Received message! Decoding" id = ser.read(1) size = ser.read(1) #print "Message size: %s Message ID: %s" %(hex(ord(size[0])),hex(ord(id[0]))) buff = ser.read(ord(size[0])) #print "Message: %s" %buff checksum = ser.read(2) if ord(id) == msgid_inv["GPS"]: xormask = struct.unpack("B", buff[55])[0] sequence = struct.unpack("H", buff[55:57])[0] numSat = struct.unpack("B", buff[48])[0] print "Mask: %s" %xormask buff = bytearray(buff) for i in range(len(buff)): buff[i] = buff[i] ^ xormask buff = str(buff) datetime = struct.unpack("I", buff[0:4])[0] latitude = struct.unpack("i", buff[4:8])[0] longitude = struct.unpack("i", buff[8:12])[0] altitude = struct.unpack("i", buff[12:16])[0] horzacc = struct.unpack("I", buff[16:20])[0] vertacc = struct.unpack("I", buff[20:24])[0] zero0 = struct.unpack("I", buff[24:28])[0] nedNV = struct.unpack("I", buff[28:32])[0] nedEV = struct.unpack("I", buff[32:36])[0] nedDV = struct.unpack("I", buff[36:40])[0] posDOP = struct.unpack("H", buff[40:42])[0] verDOP = struct.unpack("H", buff[42:44])[0] norDOP = struct.unpack("H", buff[44:46])[0] easDOP = struct.unpack("H", buff[46:48])[0] zero1 = struct.unpack("B", buff[49])[0] fixtype = struct.unpack("B", buff[50])[0] zero2 = struct.unpack("B", buff[51])[0] fixstatus = struct.unpack("B", buff[52])[0] zero3 = struct.unpack("H", buff[52:54])[0] print "%s %s" %(latitude/1e7,longitude/1e7) #print datetime print ParseDateTime(datetime) elif ord(id) == msgid_inv["MAG"]: buff = bytearray(buff) bmask = buff[4] xormask = GenMagMask(bmask) for i in range(len(buff)): if i != 4: buff[i] ^= xormask buff = str(buff) magx = struct.unpack("h", buff[0:2])[0] magy = struct.unpack("h", buff[2:4])[0] magz = struct.unpack("h", buff[4:6])[0] print "Mag(%s,%s,%s)" %(magx,magy,magz)