0
|
1 #!/usr/bin/env python
|
|
2
|
3
|
3 # Copyright (c) 2009
|
|
4 # Daniel O'Connor <darius@dons.net.au>. All rights reserved.
|
|
5 #
|
|
6 # Redistribution and use in source and binary forms, with or without
|
|
7 # modification, are permitted provided that the following conditions
|
|
8 # are met:
|
|
9 # 1. Redistributions of source code must retain the above copyright
|
|
10 # notice, this list of conditions and the following disclaimer.
|
|
11 # 2. Redistributions in binary form must reproduce the above copyright
|
|
12 # notice, this list of conditions and the following disclaimer in the
|
|
13 # documentation and/or other materials provided with the distribution.
|
|
14 #
|
|
15 # THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
|
16 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
17 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
18 # ARE DISCLAIMED. IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
|
|
19 # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
20 # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
|
21 # OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
|
22 # HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
23 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
|
24 # OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
|
25 # SUCH DAMAGE.
|
|
26
|
2
|
27 import time, struct, os
|
0
|
28
|
|
29 # SiRF at 9600 baud
|
|
30 nmea2sirf = '$PSRF100,0,9600,8,1,0*0C\r\n'
|
|
31
|
|
32 # NMEA at 4800 baud
|
|
33 # Use like so
|
|
34 # s.write(sirf.Parser.OrdLsttoStr(sirf.Parser.Encap(sirf.sirf2nmea)))
|
|
35 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 ]
|
|
36
|
|
37 #s.write(sirf.Parser.OrdLsttoStr(sirf.Parser.Encap(sirf.getver)))
|
|
38 getver = [ 0x84, 0x00 ]
|
|
39
|
|
40 # Messages from engine
|
|
41 sirfoutpktdsc = {
|
|
42 0x01 : "Reference Navigation Data",
|
|
43 0x02 : "Measured Navigation Data",
|
|
44 0x03 : "True Tracker Data",
|
|
45 0x04 : "Measured Tracking Data",
|
|
46 0x05 : "Raw Track Data",
|
|
47 0x06 : "SW Version",
|
|
48 0x07 : "Clock Status",
|
|
49 0x08 : "50 BPS Subframe Data",
|
|
50 0x09 : "Throughput",
|
|
51 0x0a : "Error ID",
|
|
52 0x0b : "Command Acknowledgment",
|
|
53 0x0c : "Command NAcknowledgment",
|
|
54 0x0d : "Visible List",
|
|
55 0x0e : "Almanac Data",
|
|
56 0x0f : "Ephemeris Data",
|
|
57 0x10 : "Test Mode 1",
|
|
58 0x11 : "Differential Corrections",
|
|
59 0x12 : "OkToSend",
|
|
60 0x13 : "Navigation Parameters",
|
|
61 0x14 : "Test Mode 2/3/4",
|
|
62 0x1b : "DGPS Status",
|
|
63 0x1c : "Nav. Lib. Measurement Data",
|
|
64 0x1d : "Nav. Lib. DGPS Data",
|
|
65 0x1e : "Nav. Lib. SV State Data",
|
|
66 0x1f : "Nav. Lib. Initialization Data",
|
|
67 0x29 : "Geodetic Navigation Data",
|
|
68 0x2d : "Raw DR Data",
|
|
69 0x2e : "Test Mode 3",
|
|
70 0x30 : "SiRFDRive-specific Class of Output Messages",
|
|
71 0x31 : "Test Mode 4 for SiRFLoc v2.x only",
|
|
72 0x32 : "SBAS Parameters",
|
|
73 0x34 : "1 PPS Time Message",
|
|
74 0x37 : "Test Mode 4",
|
|
75 0x38 : "Extended Ephemeris Data",
|
|
76 0xe1 : "SiRF internal message",
|
|
77 0xff : "Development Data"
|
|
78 }
|
|
79
|
|
80 # Messages to engine
|
|
81 sirfinpktdsc = {
|
|
82 0x35 : "Advanced Power Management",
|
|
83 0x80 : "Initialize Data Source",
|
|
84 0x81 : "Switch to NMEA Protocol",
|
|
85 0x82 : "Set Almanac (upload)",
|
|
86 0x83 : "Handle Formatted Dump Data",
|
|
87 0x84 : "Poll Software Version",
|
|
88 0x85 : "DGPS Source Control",
|
|
89 0x86 : "Set Main Serial Port",
|
|
90 0x87 : "Switch Protocol",
|
|
91 0x88 : "Mode Control",
|
|
92 0x89 : "DOP Mask Control",
|
|
93 0x8a : "DGPS Mode",
|
|
94 0x8b : "Elevation Mask",
|
|
95 0x8c : "Power Mask",
|
|
96 0x8d : "Editing Residual",
|
|
97 0x8e : "Steady-State Detection",
|
|
98 0x8f : "Static Navigation",
|
|
99 0x90 : "Poll Clock Status",
|
|
100 0x91 : "Set DGPS Serial Port",
|
|
101 0x92 : "Poll Almanac",
|
|
102 0x93 : "Poll Ephemeris",
|
|
103 0x94 : "Flash Update",
|
|
104 0x95 : "Set Ephemeris (upload)",
|
|
105 0x96 : "Switch Operating Mode",
|
|
106 0x97 : "Set TricklePower Parameters",
|
|
107 0x98 : "Poll Navigation Parameters",
|
|
108 0xa5 : "Set UART Configuration",
|
|
109 0xa6 : "Set Message Rate",
|
|
110 0xa7 : "Set Low Power Acquisition Parameters",
|
|
111 0xa8 : "Poll Command Parameters",
|
|
112 0xaa : "Set SBAS Parameters",
|
|
113 0xac : "SiRFDRive-specific",
|
|
114 0xb4 : "Marketing Software Configuration",
|
|
115 0xb6 : "Set UART Configuration",
|
|
116 0xe4 : "SiRF internal message",
|
|
117 0xe8 : "Extended Ephemeris Proprietary"
|
|
118 }
|
|
119
|
|
120 def fmtlatlong(x):
|
|
121 deg = int(x)
|
|
122 min = abs((x - deg) * 60)
|
|
123 sec = abs((min - int(min)) * 60)
|
|
124
|
2
|
125 return "%dd%dm%.3f" % (deg, int(min), sec)
|
0
|
126
|
|
127 class Parser(object):
|
|
128 def __init__(self, s = None):
|
|
129 self.buffer = []
|
|
130 self.state = 'init1'
|
|
131
|
|
132 self.fr_err = 0 # Framing error
|
|
133 self.ck_err = 0 # Checksum error
|
|
134 self.rx_cnt = 0 # Packet count
|
|
135
|
|
136 self.pktq = []
|
|
137 self.s = s
|
|
138
|
|
139 def processstr(self, data):
|
|
140 return self.process(map(ord, data))
|
|
141
|
|
142 def process(self, data):
|
|
143 pktcount = 0
|
|
144 for d in data:
|
|
145 #print "Looking at 0x%02x, state = %s" % (d, self.state)
|
|
146 if (self.state == 'init1'):
|
|
147 self.buffer = []
|
|
148 if (d != 0xa0):
|
|
149 print "Start1 framing error, got 0x%02x, expected 0xa0" % d
|
|
150 self.fr_err += 1
|
|
151 continue
|
|
152
|
|
153 self.state = 'init2'
|
|
154 elif (self.state == 'init2'):
|
|
155 if (d != 0xa2):
|
|
156 print "Start2 framing error, got 0x%02x, expected 0xa2" % d
|
|
157 self.fr_err += 1
|
|
158 self.state = 'init1'
|
|
159 continue
|
|
160
|
|
161 self.state = 'sizemsb'
|
|
162 elif (self.state == 'sizemsb'):
|
|
163 #print "Size1 - 0x%02x" % (d)
|
|
164 if d > 0x7f:
|
|
165 print "size msb too high (0x%02x)" % (d)
|
|
166 self.fr_err += 1
|
|
167 self.state = 'init1'
|
|
168 continue
|
|
169
|
|
170 self.sizemsb = d
|
|
171 self.state = 'sizelsb'
|
|
172 elif (self.state == 'sizelsb'):
|
|
173 #print "Size2 - 0x%02x" % (d)
|
|
174 self.dataleft = self.sizemsb << 8 | d
|
|
175 if self.dataleft < 1:
|
|
176 print "size is too small (0x%04x)" % (self.dataleft)
|
|
177 self.state = 'init1'
|
|
178 continue
|
|
179 if self.dataleft > 1024:
|
|
180 print "size too large (0x%04x)" % (self.dataleft)
|
|
181 self.fr_err += 1
|
|
182 self.state = 'init1'
|
|
183 continue
|
|
184 #print "Pkt size - 0x%04x" % (self.dataleft)
|
|
185 self.state = 'data'
|
|
186 elif (self.state == 'data'):
|
|
187 self.buffer.append(d)
|
|
188 self.dataleft = self.dataleft - 1
|
|
189
|
|
190 if self.dataleft == 0:
|
|
191 self.state = 'cksum1'
|
|
192
|
|
193 elif (self.state == 'cksum1'):
|
|
194 self.cksummsb = d
|
|
195 self.state = 'cksum2'
|
|
196 elif (self.state == 'cksum2'):
|
|
197 self.rxcksum = self.cksummsb << 8 | d
|
|
198 self.state = 'end1'
|
|
199 elif (self.state == 'end1'):
|
|
200 if (d != 0xb0):
|
|
201 print "End1 framing error, got 0x%02x, expected 0xb0" % d
|
|
202 self.state = 'init1'
|
|
203 self.fr_err += 1
|
|
204 continue
|
|
205
|
|
206 self.state = 'end2'
|
|
207 elif (self.state == 'end2'):
|
|
208 if (d != 0xb3):
|
|
209 print "End2 framing error, got 0x%02x, expected 0xb3" % d
|
|
210 self.fr_err += 1
|
|
211 else:
|
|
212 pktsum = reduce(lambda x, y: x + y, self.buffer) & 0x7fff
|
|
213 if (pktsum != self.rxcksum):
|
|
214 print "Checksum error: got 0x%04x, expected 0x%04x" % \
|
|
215 (self.rxcksum, pktsum)
|
|
216 print "buffer is %s" % (str(self.buffer))
|
|
217 self.state = 'init1'
|
|
218 self.ck_err += 1
|
|
219 else:
|
|
220 p = Parser.Build(self.buffer)
|
|
221 #self.pktq.append(p)
|
|
222 pktcount += 1
|
|
223 self.rx_cnt += 1
|
|
224
|
|
225 self.state = 'init1'
|
|
226 else:
|
|
227 print "Invalid state %s! Resetting" % (self.state)
|
|
228 self.state = 'init1'
|
|
229
|
|
230 return pktcount
|
|
231
|
|
232 def dumpmsgs(self, s):
|
|
233 s.setTimeout(0.1)
|
|
234 while True:
|
|
235 self.processstr(s.read(100))
|
|
236
|
|
237 @classmethod
|
|
238 def Encap(self, data):
|
|
239 cksum = reduce(lambda x, y: x + y, data)
|
|
240 dlen = len(data)
|
|
241 out = [ 0xa0, 0xa2 ]
|
|
242 out.append((dlen & 0xff00) >> 8)
|
|
243 out.append(dlen & 0xff)
|
|
244 out.extend(data)
|
|
245 out.append((cksum & 0xff00) >> 8)
|
|
246 out.append(cksum & 0xff)
|
|
247 out.extend([0xb0, 0xb3])
|
|
248 return out
|
|
249
|
|
250 @classmethod
|
|
251 def OrdLsttoStr(self, data):
|
|
252 return reduce(lambda x, y: x + y, map(chr, data))
|
|
253
|
|
254 @classmethod
|
|
255 def Build(self, data):
|
|
256 t = time.time()
|
|
257 t1 = time.strftime("%Y/%m/%d %H:%M:%S", time.localtime(t))
|
|
258 t2 = (t - int(t)) * 1e3
|
|
259 tfmt = "%s.%03d" % (t1, t2)
|
|
260 if data[0] in sirfoutpktdsc:
|
|
261 print "%s: Got out packet 0x%02x : %s" % (tfmt, data[0], sirfoutpktdsc[data[0]])
|
|
262 elif data[0] in sirfinpktdsc:
|
|
263 print "%s: Got in packet 0x%02x : %s" % (tfmt, data[0], sirfinpktdsc[data[0]])
|
|
264 else:
|
|
265 print "%s: Unknown packet type 0x%02x" % (tfmt, data[0])
|
|
266 print "Payload - " + str(data[1:])
|
|
267
|
|
268 if data[0] == 0x02:
|
|
269 fmt = '>iiihhhBBBhIB'
|
|
270 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1]))
|
|
271 (xpos, ypos, zpos, xvel, yvel, zvel, mode1, hdop, mode2, gpsweek, gpstow, nsats) = \
|
|
272 struct.unpack(fmt, datastr)
|
|
273
|
|
274 satprn = []
|
|
275 for i in xrange(nsats):
|
2
|
276 satprn.append(data[struct.calcsize(fmt) - 1 + i])
|
0
|
277
|
|
278 xvel = float(xvel) / 8
|
|
279 yvel = float(yvel) / 8
|
|
280 zvel = float(zvel) / 8
|
2
|
281 hdop = float(hdop) / 5
|
|
282 gpstow = float(gpstow) / 100
|
|
283
|
|
284 print " Position: X: %d m, Y: %d m, Z: %d m" % (xpos, ypos, zpos)
|
|
285 print " Velocity: X: %.2f m/s, Y: %.2f m/s, Z: %.2f m/s" % (xvel, yvel, zvel)
|
|
286 print " HDOP: %.1f, Week: %d, TOW: %.2f seconds" % (hdop, gpsweek, gpstow)
|
0
|
287 elif data[0] == 0x06:
|
|
288 nulidx = data.index(0)
|
|
289 print " SW Ver : %s" % (reduce(lambda x, y: x + y, map(chr, data[1:nulidx])))
|
|
290 elif data[0] == 0x0a:
|
|
291 errid = data[1] << 8 | data[2]
|
|
292 dlen = (data[3] << 8 | data[4]) * 4
|
|
293 print " Error ID : 0x%04x" % (errid)
|
|
294 if dlen > 0:
|
|
295 print " Length : 0x%04x" % (dlen)
|
|
296 print " Payload : %s" % (data[5:])
|
|
297 elif data[0] == 0x0b:
|
|
298 print " Cmd Ack : 0x%02x" % (data[1])
|
|
299 elif data[0] == 0x0c:
|
|
300 print " Cmd NAck : 0x%02x" % (data[1])
|
|
301 elif data[0] == 0x29:
|
|
302 fixtype = {
|
|
303 0 : "none",
|
|
304 1 : "1-SV KF",
|
|
305 2 : "2-SV KF",
|
|
306 3 : "3-SV KF",
|
|
307 4 : "4+-SV KF",
|
|
308 5 : "2D",
|
|
309 6 : "3D",
|
|
310 7 : "DR"
|
|
311 }
|
|
312 fmt = '>HHHIHBBBBHIiiiiBHHHHHIIIHIIIIIHHBBB'
|
|
313 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1]))
|
|
314 (navval, navtype, ewn, tow, year, month, day, hour, minute, second, satlst,
|
|
315 latitude, longitude, alt_elip, alt_msl, datum, sog, cog, magvar, climbrate,
|
|
316 headrate, estHPE, estVPE, estTE, estHVE, clockbias, CBerr, clockdrift,
|
|
317 CDerr, distance, distanceErr, headErr, numsvs, hdop, addmodeinfo) = \
|
|
318 struct.unpack(fmt, datastr)
|
|
319 tow = float(tow) / 1e3
|
2
|
320 second = float(second) / 1e3
|
0
|
321 latitude = float(latitude) / 1e7
|
|
322 longitude = float(longitude) / 1e7
|
|
323 alt_elip = float(alt_elip) / 1e2
|
|
324 alt_msl = float(alt_msl) / 1e2
|
|
325 sog = float(sog) / 1e2
|
|
326 cog = float(cog) / 1e2
|
|
327 climbrate = float(climbrate) / 1e2
|
|
328 headrate = float(headrate) / 1e2
|
|
329 estHPE = float(estHPE) / 1e2
|
|
330 estVPE = float(estVPE) / 1e2
|
|
331 estTE = float(estTE) / 1e2
|
|
332 estHVE = float(estHVE) / 1e2
|
|
333 clockbias = float(clockbias) / 1e2
|
|
334 CBerr = float(CBerr) / 1e2
|
|
335 clockdrift = float(clockdrift) / 1e2
|
|
336 CDerr = float(CDerr) / 1e2
|
|
337 headErr = float(headErr) / 1e2
|
|
338 hdop = float(hdop) / 5
|
|
339
|
2
|
340 utctime = timegm((year, month, day, hour, minute, int(second)))
|
|
341 utctime = utctime + (second - int(second))
|
|
342 print " Fix : %s, Sats : %d, Lat: %.06f, Long: %.06f, Alt: %.02fm" % \
|
|
343 (fixtype[navtype & 0x7], numsvs, latitude, longitude, alt_msl)
|
|
344 print " Date : %04d/%02d/%02d %02d:%02d:%02d.%03d" % \
|
|
345 (year, month, day, hour, month, int(second), int((second - int(second)) * 1000))
|
|
346 print " Epoch time : %.3f, Delta %.3f" % (utctime, abs(time.time() - utctime))
|
|
347 elif data[0] == 0x34:
|
|
348 fmt = '>BBBBBHHIB'
|
|
349 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1]))
|
|
350 (hour, minute, second, day, month, year, offsetint, offsetfrac, status) = \
|
|
351 struct.unpack(fmt, datastr)
|
|
352 offset = offsetint + float(offsetfrac) / 1e9
|
|
353 print " PPS : %04d/%02d/%02d %02d:%02d:%02d, Offset : %.9f, Status : 0x%02x" % \
|
|
354 (year, month, day, hour, minute, second, offset, status)
|
0
|
355 elif data[0] == 0xa6:
|
|
356 print " Message rate : MID 0x%02x, rate 0x%02x" % (data[2], data[3])
|
|
357
|
|
358 def enablemsgs(s):
|
|
359 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
360 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x04, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
361 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x07, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
362 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x09, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
363 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x0d, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
364 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
365 #s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x34, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
366 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x1b, 0x01, 0x00, 0x00, 0x00, 0x00])))
|
|
367
|
|
368
|
|
369 def disablemsgs(s):
|
|
370 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
371 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
372 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
373 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
374 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
375 #s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x29, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
376 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
377 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x34, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
378 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00])))
|
|
379
|
|
380
|
2
|
381 def timegm(tuple):
|
|
382 oldtz = None
|
|
383 if 'TZ' in os.environ:
|
|
384 oldtz = os.environ['TZ']
|
|
385 os.environ['TZ'] = 'UTC'
|
|
386
|
|
387 t = time.mktime(tuple + (0, 0, 0))
|
|
388 if oldtz == None:
|
|
389 del os.environ['TZ']
|
|
390 else:
|
|
391 os.environ['TZ'] = oldtz
|
|
392
|
|
393 return t
|