|
0
|
1 #!/usr/bin/env python
|
|
|
2
|
|
|
3 import time, struct
|
|
|
4
|
|
|
5 # SiRF at 9600 baud
|
|
|
6 nmea2sirf = '$PSRF100,0,9600,8,1,0*0C\r\n'
|
|
|
7
|
|
|
8 # NMEA at 4800 baud
|
|
|
9 # Use like so
|
|
|
10 # s.write(sirf.Parser.OrdLsttoStr(sirf.Parser.Encap(sirf.sirf2nmea)))
|
|
|
11 sirf2nmea = [ 0x81, 0x02, 0x01, 0x01, 0x00, 0x00, 0x01, 0x01, 0x05, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x12, 0xc0 ]
|
|
|
12
|
|
|
13 #s.write(sirf.Parser.OrdLsttoStr(sirf.Parser.Encap(sirf.getver)))
|
|
|
14 getver = [ 0x84, 0x00 ]
|
|
|
15
|
|
|
16 # Messages from engine
|
|
|
17 sirfoutpktdsc = {
|
|
|
18 0x01 : "Reference Navigation Data",
|
|
|
19 0x02 : "Measured Navigation Data",
|
|
|
20 0x03 : "True Tracker Data",
|
|
|
21 0x04 : "Measured Tracking Data",
|
|
|
22 0x05 : "Raw Track Data",
|
|
|
23 0x06 : "SW Version",
|
|
|
24 0x07 : "Clock Status",
|
|
|
25 0x08 : "50 BPS Subframe Data",
|
|
|
26 0x09 : "Throughput",
|
|
|
27 0x0a : "Error ID",
|
|
|
28 0x0b : "Command Acknowledgment",
|
|
|
29 0x0c : "Command NAcknowledgment",
|
|
|
30 0x0d : "Visible List",
|
|
|
31 0x0e : "Almanac Data",
|
|
|
32 0x0f : "Ephemeris Data",
|
|
|
33 0x10 : "Test Mode 1",
|
|
|
34 0x11 : "Differential Corrections",
|
|
|
35 0x12 : "OkToSend",
|
|
|
36 0x13 : "Navigation Parameters",
|
|
|
37 0x14 : "Test Mode 2/3/4",
|
|
|
38 0x1b : "DGPS Status",
|
|
|
39 0x1c : "Nav. Lib. Measurement Data",
|
|
|
40 0x1d : "Nav. Lib. DGPS Data",
|
|
|
41 0x1e : "Nav. Lib. SV State Data",
|
|
|
42 0x1f : "Nav. Lib. Initialization Data",
|
|
|
43 0x29 : "Geodetic Navigation Data",
|
|
|
44 0x2d : "Raw DR Data",
|
|
|
45 0x2e : "Test Mode 3",
|
|
|
46 0x30 : "SiRFDRive-specific Class of Output Messages",
|
|
|
47 0x31 : "Test Mode 4 for SiRFLoc v2.x only",
|
|
|
48 0x32 : "SBAS Parameters",
|
|
|
49 0x34 : "1 PPS Time Message",
|
|
|
50 0x37 : "Test Mode 4",
|
|
|
51 0x38 : "Extended Ephemeris Data",
|
|
|
52 0xe1 : "SiRF internal message",
|
|
|
53 0xff : "Development Data"
|
|
|
54 }
|
|
|
55
|
|
|
56 # Messages to engine
|
|
|
57 sirfinpktdsc = {
|
|
|
58 0x35 : "Advanced Power Management",
|
|
|
59 0x80 : "Initialize Data Source",
|
|
|
60 0x81 : "Switch to NMEA Protocol",
|
|
|
61 0x82 : "Set Almanac (upload)",
|
|
|
62 0x83 : "Handle Formatted Dump Data",
|
|
|
63 0x84 : "Poll Software Version",
|
|
|
64 0x85 : "DGPS Source Control",
|
|
|
65 0x86 : "Set Main Serial Port",
|
|
|
66 0x87 : "Switch Protocol",
|
|
|
67 0x88 : "Mode Control",
|
|
|
68 0x89 : "DOP Mask Control",
|
|
|
69 0x8a : "DGPS Mode",
|
|
|
70 0x8b : "Elevation Mask",
|
|
|
71 0x8c : "Power Mask",
|
|
|
72 0x8d : "Editing Residual",
|
|
|
73 0x8e : "Steady-State Detection",
|
|
|
74 0x8f : "Static Navigation",
|
|
|
75 0x90 : "Poll Clock Status",
|
|
|
76 0x91 : "Set DGPS Serial Port",
|
|
|
77 0x92 : "Poll Almanac",
|
|
|
78 0x93 : "Poll Ephemeris",
|
|
|
79 0x94 : "Flash Update",
|
|
|
80 0x95 : "Set Ephemeris (upload)",
|
|
|
81 0x96 : "Switch Operating Mode",
|
|
|
82 0x97 : "Set TricklePower Parameters",
|
|
|
83 0x98 : "Poll Navigation Parameters",
|
|
|
84 0xa5 : "Set UART Configuration",
|
|
|
85 0xa6 : "Set Message Rate",
|
|
|
86 0xa7 : "Set Low Power Acquisition Parameters",
|
|
|
87 0xa8 : "Poll Command Parameters",
|
|
|
88 0xaa : "Set SBAS Parameters",
|
|
|
89 0xac : "SiRFDRive-specific",
|
|
|
90 0xb4 : "Marketing Software Configuration",
|
|
|
91 0xb6 : "Set UART Configuration",
|
|
|
92 0xe4 : "SiRF internal message",
|
|
|
93 0xe8 : "Extended Ephemeris Proprietary"
|
|
|
94 }
|
|
|
95
|
|
|
96 def fmtlatlong(x):
|
|
|
97 deg = int(x)
|
|
|
98 min = abs((x - deg) * 60)
|
|
|
99 sec = abs((min - int(min)) * 60)
|
|
|
100
|
|
|
101 return "%d d %d m %.3f s" % (deg, int(min), sec)
|
|
|
102
|
|
|
103 class Parser(object):
|
|
|
104 def __init__(self, s = None):
|
|
|
105 self.buffer = []
|
|
|
106 self.state = 'init1'
|
|
|
107
|
|
|
108 self.fr_err = 0 # Framing error
|
|
|
109 self.ck_err = 0 # Checksum error
|
|
|
110 self.rx_cnt = 0 # Packet count
|
|
|
111
|
|
|
112 self.pktq = []
|
|
|
113 self.s = s
|
|
|
114
|
|
|
115 def processstr(self, data):
|
|
|
116 return self.process(map(ord, data))
|
|
|
117
|
|
|
118 def process(self, data):
|
|
|
119 pktcount = 0
|
|
|
120 for d in data:
|
|
|
121 #print "Looking at 0x%02x, state = %s" % (d, self.state)
|
|
|
122 if (self.state == 'init1'):
|
|
|
123 self.buffer = []
|
|
|
124 if (d != 0xa0):
|
|
|
125 print "Start1 framing error, got 0x%02x, expected 0xa0" % d
|
|
|
126 self.fr_err += 1
|
|
|
127 continue
|
|
|
128
|
|
|
129 self.state = 'init2'
|
|
|
130 elif (self.state == 'init2'):
|
|
|
131 if (d != 0xa2):
|
|
|
132 print "Start2 framing error, got 0x%02x, expected 0xa2" % d
|
|
|
133 self.fr_err += 1
|
|
|
134 self.state = 'init1'
|
|
|
135 continue
|
|
|
136
|
|
|
137 self.state = 'sizemsb'
|
|
|
138 elif (self.state == 'sizemsb'):
|
|
|
139 #print "Size1 - 0x%02x" % (d)
|
|
|
140 if d > 0x7f:
|
|
|
141 print "size msb too high (0x%02x)" % (d)
|
|
|
142 self.fr_err += 1
|
|
|
143 self.state = 'init1'
|
|
|
144 continue
|
|
|
145
|
|
|
146 self.sizemsb = d
|
|
|
147 self.state = 'sizelsb'
|
|
|
148 elif (self.state == 'sizelsb'):
|
|
|
149 #print "Size2 - 0x%02x" % (d)
|
|
|
150 self.dataleft = self.sizemsb << 8 | d
|
|
|
151 if self.dataleft < 1:
|
|
|
152 print "size is too small (0x%04x)" % (self.dataleft)
|
|
|
153 self.state = 'init1'
|
|
|
154 continue
|
|
|
155 if self.dataleft > 1024:
|
|
|
156 print "size too large (0x%04x)" % (self.dataleft)
|
|
|
157 self.fr_err += 1
|
|
|
158 self.state = 'init1'
|
|
|
159 continue
|
|
|
160 #print "Pkt size - 0x%04x" % (self.dataleft)
|
|
|
161 self.state = 'data'
|
|
|
162 elif (self.state == 'data'):
|
|
|
163 self.buffer.append(d)
|
|
|
164 self.dataleft = self.dataleft - 1
|
|
|
165
|
|
|
166 if self.dataleft == 0:
|
|
|
167 self.state = 'cksum1'
|
|
|
168
|
|
|
169 elif (self.state == 'cksum1'):
|
|
|
170 self.cksummsb = d
|
|
|
171 self.state = 'cksum2'
|
|
|
172 elif (self.state == 'cksum2'):
|
|
|
173 self.rxcksum = self.cksummsb << 8 | d
|
|
|
174 self.state = 'end1'
|
|
|
175 elif (self.state == 'end1'):
|
|
|
176 if (d != 0xb0):
|
|
|
177 print "End1 framing error, got 0x%02x, expected 0xb0" % d
|
|
|
178 self.state = 'init1'
|
|
|
179 self.fr_err += 1
|
|
|
180 continue
|
|
|
181
|
|
|
182 self.state = 'end2'
|
|
|
183 elif (self.state == 'end2'):
|
|
|
184 if (d != 0xb3):
|
|
|
185 print "End2 framing error, got 0x%02x, expected 0xb3" % d
|
|
|
186 self.fr_err += 1
|
|
|
187 else:
|
|
|
188 pktsum = reduce(lambda x, y: x + y, self.buffer) & 0x7fff
|
|
|
189 if (pktsum != self.rxcksum):
|
|
|
190 print "Checksum error: got 0x%04x, expected 0x%04x" % \
|
|
|
191 (self.rxcksum, pktsum)
|
|
|
192 print "buffer is %s" % (str(self.buffer))
|
|
|
193 self.state = 'init1'
|
|
|
194 self.ck_err += 1
|
|
|
195 else:
|
|
|
196 p = Parser.Build(self.buffer)
|
|
|
197 #self.pktq.append(p)
|
|
|
198 pktcount += 1
|
|
|
199 self.rx_cnt += 1
|
|
|
200
|
|
|
201 self.state = 'init1'
|
|
|
202 else:
|
|
|
203 print "Invalid state %s! Resetting" % (self.state)
|
|
|
204 self.state = 'init1'
|
|
|
205
|
|
|
206 return pktcount
|
|
|
207
|
|
|
208 def dumpmsgs(self, s):
|
|
|
209 s.setTimeout(0.1)
|
|
|
210 while True:
|
|
|
211 self.processstr(s.read(100))
|
|
|
212
|
|
|
213 @classmethod
|
|
|
214 def Encap(self, data):
|
|
|
215 cksum = reduce(lambda x, y: x + y, data)
|
|
|
216 dlen = len(data)
|
|
|
217 out = [ 0xa0, 0xa2 ]
|
|
|
218 out.append((dlen & 0xff00) >> 8)
|
|
|
219 out.append(dlen & 0xff)
|
|
|
220 out.extend(data)
|
|
|
221 out.append((cksum & 0xff00) >> 8)
|
|
|
222 out.append(cksum & 0xff)
|
|
|
223 out.extend([0xb0, 0xb3])
|
|
|
224 return out
|
|
|
225
|
|
|
226 @classmethod
|
|
|
227 def OrdLsttoStr(self, data):
|
|
|
228 return reduce(lambda x, y: x + y, map(chr, data))
|
|
|
229
|
|
|
230 @classmethod
|
|
|
231 def Build(self, data):
|
|
|
232 t = time.time()
|
|
|
233 t1 = time.strftime("%Y/%m/%d %H:%M:%S", time.localtime(t))
|
|
|
234 t2 = (t - int(t)) * 1e3
|
|
|
235 tfmt = "%s.%03d" % (t1, t2)
|
|
|
236 if data[0] in sirfoutpktdsc:
|
|
|
237 print "%s: Got out packet 0x%02x : %s" % (tfmt, data[0], sirfoutpktdsc[data[0]])
|
|
|
238 elif data[0] in sirfinpktdsc:
|
|
|
239 print "%s: Got in packet 0x%02x : %s" % (tfmt, data[0], sirfinpktdsc[data[0]])
|
|
|
240 else:
|
|
|
241 print "%s: Unknown packet type 0x%02x" % (tfmt, data[0])
|
|
|
242 print "Payload - " + str(data[1:])
|
|
|
243
|
|
|
244 if data[0] == 0x02:
|
|
|
245 fmt = '>iiihhhBBBhIB'
|
|
|
246 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1]))
|
|
|
247 (xpos, ypos, zpos, xvel, yvel, zvel, mode1, hdop, mode2, gpsweek, gpstow, nsats) = \
|
|
|
248 struct.unpack(fmt, datastr)
|
|
|
249
|
|
|
250 satprn = []
|
|
|
251 for i in xrange(nsats):
|
|
|
252 satprn.append(data[struct.calcsize(fmt) + i])
|
|
|
253
|
|
|
254 xvel = float(xvel) / 8
|
|
|
255 yvel = float(yvel) / 8
|
|
|
256 zvel = float(zvel) / 8
|
|
|
257 print " Position: X: %d, Y: %d, Z: %d" % (xpos, ypos, zpos)
|
|
|
258 print " Velocity: X: %.2f, Y: %.2f, Z: %.2f" % (xvel, yvel, zvel)
|
|
|
259 elif data[0] == 0x06:
|
|
|
260 nulidx = data.index(0)
|
|
|
261 print " SW Ver : %s" % (reduce(lambda x, y: x + y, map(chr, data[1:nulidx])))
|
|
|
262 elif data[0] == 0x0a:
|
|
|
263 errid = data[1] << 8 | data[2]
|
|
|
264 dlen = (data[3] << 8 | data[4]) * 4
|
|
|
265 print " Error ID : 0x%04x" % (errid)
|
|
|
266 if dlen > 0:
|
|
|
267 print " Length : 0x%04x" % (dlen)
|
|
|
268 print " Payload : %s" % (data[5:])
|
|
|
269 elif data[0] == 0x0b:
|
|
|
270 print " Cmd Ack : 0x%02x" % (data[1])
|
|
|
271 elif data[0] == 0x0c:
|
|
|
272 print " Cmd NAck : 0x%02x" % (data[1])
|
|
|
273 elif data[0] == 0x29:
|
|
|
274 fixtype = {
|
|
|
275 0 : "none",
|
|
|
276 1 : "1-SV KF",
|
|
|
277 2 : "2-SV KF",
|
|
|
278 3 : "3-SV KF",
|
|
|
279 4 : "4+-SV KF",
|
|
|
280 5 : "2D",
|
|
|
281 6 : "3D",
|
|
|
282 7 : "DR"
|
|
|
283 }
|
|
|
284 fmt = '>HHHIHBBBBHIiiiiBHHHHHIIIHIIIIIHHBBB'
|
|
|
285 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1]))
|
|
|
286 (navval, navtype, ewn, tow, year, month, day, hour, minute, second, satlst,
|
|
|
287 latitude, longitude, alt_elip, alt_msl, datum, sog, cog, magvar, climbrate,
|
|
|
288 headrate, estHPE, estVPE, estTE, estHVE, clockbias, CBerr, clockdrift,
|
|
|
289 CDerr, distance, distanceErr, headErr, numsvs, hdop, addmodeinfo) = \
|
|
|
290 struct.unpack(fmt, datastr)
|
|
|
291 tow = float(tow) / 1e3
|
|
|
292 latitude = float(latitude) / 1e7
|
|
|
293 longitude = float(longitude) / 1e7
|
|
|
294 alt_elip = float(alt_elip) / 1e2
|
|
|
295 alt_msl = float(alt_msl) / 1e2
|
|
|
296 sog = float(sog) / 1e2
|
|
|
297 cog = float(cog) / 1e2
|
|
|
298 climbrate = float(climbrate) / 1e2
|
|
|
299 headrate = float(headrate) / 1e2
|
|
|
300 estHPE = float(estHPE) / 1e2
|
|
|
301 estVPE = float(estVPE) / 1e2
|
|
|
302 estTE = float(estTE) / 1e2
|
|
|
303 estHVE = float(estHVE) / 1e2
|
|
|
304 clockbias = float(clockbias) / 1e2
|
|
|
305 CBerr = float(CBerr) / 1e2
|
|
|
306 clockdrift = float(clockdrift) / 1e2
|
|
|
307 CDerr = float(CDerr) / 1e2
|
|
|
308 headErr = float(headErr) / 1e2
|
|
|
309 hdop = float(hdop) / 5
|
|
|
310
|
|
|
311 print " Fix : %s, Sats : %d, Lat: %s, Long: %s, Alt: %.2fm" % \
|
|
|
312 (fixtype[navtype & 0x7], numsvs, fmtlatlong(latitude), fmtlatlong(longitude), \
|
|
|
313 alt_msl)
|
|
|
314 elif data[0] == 0xa6:
|
|
|
315 print " Message rate : MID 0x%02x, rate 0x%02x" % (data[2], data[3])
|
|
|
316
|
|
|
317 def enablemsgs(s):
|
|
|
318 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
319 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x04, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
320 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x07, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
321 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x09, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
322 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x0d, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
323 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
324 #s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x34, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
325 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x1b, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
|
326
|
|
|
327
|
|
|
328 def disablemsgs(s):
|
|
|
329 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
330 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
331 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
332 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
333 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
334 #s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x29, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
335 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
336 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x34, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
337 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
|
338
|
|
|
339
|
