喜歡 0
分支 0

最後活躍 1 month ago

Naza-M Lite GPS Interface

Lucas Teske 已修改 11 years ago. 還原成這個修訂版本

1 file changed, 126 insertions

gps.py(檔案已創建)

@@ -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 +
上一頁 下一頁

由 Opengist 提供支持 ⋅ Load: 17ms⋅

繁體中文