Revision of gps.py

Lucas Teske gist felülvizsgálása 11 years ago. Revízióhoz ugrás

1 file changed, 126 insertions

gps.py(fájl létrehozva)

		@@ -0,0 +1,126 @@
1	+	#!/usr/bin/env python
2	+
3	+
4	+	import serial
5	+	import struct
6	+
7	+	ser = serial.Serial("/dev/ttyUSB0", 115200, timeout=2)
8	+
9	+	msgid = {
10	+	0x10 : "GPS",
11	+	0x20 : "MAG"
12	+	}
13	+
14	+	msgid_inv = {
15	+	"GPS" : 0x10,
16	+	"MAG" : 0x20
17	+	}
18	+
19	+	fixtype = {
20	+	0x00 : "No Fix",
21	+	0x02 : "2D Lock",
22	+	0x03 : "3D Lock"
23	+	}
24	+
25	+	def ParseDateTime(data):
26	+	seconds = (data & 0x3F ) >> 0
27	+	minutes = (data & 0xFC0 ) >> 6
28	+	hours = (data & 0xF000 ) >> 12
29	+	day = (data & 0x1F0000 ) >> 16
30	+	month = (data & 0x1E00000 ) >> 21
31	+	year = (data & 0xFE000000 ) >> 25
32	+	return {"seconds":seconds,"minutes":minutes,"hours":hours,"day":day,"month":month,"year":year}
33	+
34	+	def bin(s):
35	+	return str(s) if s<=1 else bin(s>>1) + str(s&1)
36	+
37	+	def GenMagMask(b9):
38	+	b9b = bin(b9).rjust(8,"0")
39	+	_0 = int(b9b[7])
40	+	_1 = int(b9b[6])
41	+	_2 = int(b9b[5])
42	+	_3 = int(b9b[4])
43	+	_4 = int(b9b[3])
44	+	_5 = int(b9b[2])
45	+	_6 = int(b9b[1])
46	+	_7 = int(b9b[0])
47	+	mask = [0,0,0,0,0,0,0,0]
48	+	mask[7] = _0 ^ _4
49	+	mask[6] = _1 ^ _5
50	+	mask[5] = _2 ^ _6
51	+	mask[4] = _3 ^ _7 ^ _0
52	+	mask[3] = _1
53	+	mask[2] = _2
54	+	mask[1] = _3
55	+	mask[0] = _4 ^ _0
56	+	mask = int("".join([str(x) for x in mask]), 2)
57	+	return mask
58	+
59	+	while True:
60	+	buff = ser.read(2)
61	+	if buff == "\x55\xAA":
62	+	#print "Received message! Decoding"
63	+	id = ser.read(1)
64	+	size = ser.read(1)
65	+	#print "Message size: %s Message ID: %s" %(hex(ord(size[0])),hex(ord(id[0])))
66	+	buff = ser.read(ord(size[0]))
67	+	#print "Message: %s" %buff
68	+	checksum = ser.read(2)
69	+	if ord(id) == msgid_inv["GPS"]:
70	+	xormask = struct.unpack("B", buff[55])[0]
71	+	sequence = struct.unpack("H", buff[55:57])[0]
72	+	numSat = struct.unpack("B", buff[48])[0]
73	+
74	+	print "Mask: %s" %xormask
75	+
76	+	buff = bytearray(buff)
77	+	for i in range(len(buff)):
78	+	buff[i] = buff[i] ^ xormask
79	+	buff = str(buff)
80	+
81	+	datetime = struct.unpack("I", buff[0:4])[0]
82	+
83	+	latitude = struct.unpack("i", buff[4:8])[0]
84	+	longitude = struct.unpack("i", buff[8:12])[0]
85	+	altitude = struct.unpack("i", buff[12:16])[0]
86	+
87	+	horzacc = struct.unpack("I", buff[16:20])[0]
88	+	vertacc = struct.unpack("I", buff[20:24])[0]
89	+
90	+	zero0 = struct.unpack("I", buff[24:28])[0]
91	+
92	+	nedNV = struct.unpack("I", buff[28:32])[0]
93	+	nedEV = struct.unpack("I", buff[32:36])[0]
94	+	nedDV = struct.unpack("I", buff[36:40])[0]
95	+
96	+	posDOP = struct.unpack("H", buff[40:42])[0]
97	+	verDOP = struct.unpack("H", buff[42:44])[0]
98	+	norDOP = struct.unpack("H", buff[44:46])[0]
99	+	easDOP = struct.unpack("H", buff[46:48])[0]
100	+
101	+	zero1 = struct.unpack("B", buff[49])[0]
102	+	fixtype = struct.unpack("B", buff[50])[0]
103	+	zero2 = struct.unpack("B", buff[51])[0]
104	+	fixstatus = struct.unpack("B", buff[52])[0]
105	+
106	+	zero3 = struct.unpack("H", buff[52:54])[0]
107	+
108	+
109	+	print "%s %s" %(latitude/1e7,longitude/1e7)
110	+	#print datetime
111	+	print ParseDateTime(datetime)
112	+	elif ord(id) == msgid_inv["MAG"]:
113	+	buff = bytearray(buff)
114	+	bmask = buff[4]
115	+	xormask = GenMagMask(bmask)
116	+	for i in range(len(buff)):
117	+	if i != 4:
118	+	buff[i] ^= xormask
119	+	buff = str(buff)
120	+	magx = struct.unpack("h", buff[0:2])[0]
121	+	magy = struct.unpack("h", buff[2:4])[0]
122	+	magz = struct.unpack("h", buff[4:6])[0]
123	+	print "Mag(%s,%s,%s)" %(magx,magy,magz)
124	+
125	+
126	+

Újabb Régebbi