Mercurial > ~darius > hgwebdir.cgi > sirf

comparison sirf.py @ 0:6503256a3fc4

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Prototype code for parsing SiRF messages. Seems to work, but needs fleshing out for actual use.
author darius@Inchoate
date Sun, 22 Feb 2009 21:26:49 +1030
parents
children d6d9fba5464d
comparison
equal deleted inserted replaced
-1:000000000000 0:6503256a3fc4
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