Mercurial > ~darius > hgwebdir.cgi > sirf
annotate sirf.py @ 5:f3547b32c7c5 default tip
Don't set timeout in parser class otherwise we can't use it for FIFO files.
| author | Daniel O'Connor <darius@dons.net.au> |
|---|---|
| date | Fri, 02 Aug 2019 12:14:26 +0930 |
| parents | 2fde8c382dae |
| children |
| rev | line source |
|---|---|
| 0 | 1 #!/usr/bin/env python |
| 2 | |
|
5
f3547b32c7c5
Don't set timeout in parser class otherwise we can't use it for FIFO files.
Daniel O'Connor <darius@dons.net.au>
parents:
4
diff
changeset
|
3 # Copyright (c) 2019 |
| 3 | 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 while True: | |
| 234 self.processstr(s.read(100)) | |
| 235 | |
| 236 @classmethod | |
| 237 def Encap(self, data): | |
| 238 cksum = reduce(lambda x, y: x + y, data) | |
| 239 dlen = len(data) | |
| 240 out = [ 0xa0, 0xa2 ] | |
| 241 out.append((dlen & 0xff00) >> 8) | |
| 242 out.append(dlen & 0xff) | |
| 243 out.extend(data) | |
|
4
2fde8c382dae
SiRF uses a 15 bit checksum, see section 1-2
Daniel O'Connor <darius@dons.net.au>
parents:
3
diff
changeset
|
244 out.append((cksum & 0x7f00) >> 8) # 15 bit checksum |
| 0 | 245 out.append(cksum & 0xff) |
| 246 out.extend([0xb0, 0xb3]) | |
| 247 return out | |
| 248 | |
| 249 @classmethod | |
| 250 def OrdLsttoStr(self, data): | |
| 251 return reduce(lambda x, y: x + y, map(chr, data)) | |
| 252 | |
| 253 @classmethod | |
| 254 def Build(self, data): | |
| 255 t = time.time() | |
| 256 t1 = time.strftime("%Y/%m/%d %H:%M:%S", time.localtime(t)) | |
| 257 t2 = (t - int(t)) * 1e3 | |
| 258 tfmt = "%s.%03d" % (t1, t2) | |
| 259 if data[0] in sirfoutpktdsc: | |
| 260 print "%s: Got out packet 0x%02x : %s" % (tfmt, data[0], sirfoutpktdsc[data[0]]) | |
| 261 elif data[0] in sirfinpktdsc: | |
| 262 print "%s: Got in packet 0x%02x : %s" % (tfmt, data[0], sirfinpktdsc[data[0]]) | |
| 263 else: | |
| 264 print "%s: Unknown packet type 0x%02x" % (tfmt, data[0]) | |
| 265 print "Payload - " + str(data[1:]) | |
| 266 | |
| 267 if data[0] == 0x02: | |
| 268 fmt = '>iiihhhBBBhIB' | |
| 269 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1])) | |
| 270 (xpos, ypos, zpos, xvel, yvel, zvel, mode1, hdop, mode2, gpsweek, gpstow, nsats) = \ | |
| 271 struct.unpack(fmt, datastr) | |
| 272 | |
| 273 satprn = [] | |
| 274 for i in xrange(nsats): | |
| 2 | 275 satprn.append(data[struct.calcsize(fmt) - 1 + i]) |
| 0 | 276 |
| 277 xvel = float(xvel) / 8 | |
| 278 yvel = float(yvel) / 8 | |
| 279 zvel = float(zvel) / 8 | |
| 2 | 280 hdop = float(hdop) / 5 |
| 281 gpstow = float(gpstow) / 100 | |
| 282 | |
| 283 print " Position: X: %d m, Y: %d m, Z: %d m" % (xpos, ypos, zpos) | |
| 284 print " Velocity: X: %.2f m/s, Y: %.2f m/s, Z: %.2f m/s" % (xvel, yvel, zvel) | |
| 285 print " HDOP: %.1f, Week: %d, TOW: %.2f seconds" % (hdop, gpsweek, gpstow) | |
| 0 | 286 elif data[0] == 0x06: |
| 287 nulidx = data.index(0) | |
| 288 print " SW Ver : %s" % (reduce(lambda x, y: x + y, map(chr, data[1:nulidx]))) | |
| 289 elif data[0] == 0x0a: | |
| 290 errid = data[1] << 8 | data[2] | |
| 291 dlen = (data[3] << 8 | data[4]) * 4 | |
| 292 print " Error ID : 0x%04x" % (errid) | |
| 293 if dlen > 0: | |
| 294 print " Length : 0x%04x" % (dlen) | |
| 295 print " Payload : %s" % (data[5:]) | |
| 296 elif data[0] == 0x0b: | |
| 297 print " Cmd Ack : 0x%02x" % (data[1]) | |
| 298 elif data[0] == 0x0c: | |
| 299 print " Cmd NAck : 0x%02x" % (data[1]) | |
| 300 elif data[0] == 0x29: | |
| 301 fixtype = { | |
| 302 0 : "none", | |
| 303 1 : "1-SV KF", | |
| 304 2 : "2-SV KF", | |
| 305 3 : "3-SV KF", | |
| 306 4 : "4+-SV KF", | |
| 307 5 : "2D", | |
| 308 6 : "3D", | |
| 309 7 : "DR" | |
| 310 } | |
| 311 fmt = '>HHHIHBBBBHIiiiiBHHHHHIIIHIIIIIHHBBB' | |
| 312 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1])) | |
| 313 (navval, navtype, ewn, tow, year, month, day, hour, minute, second, satlst, | |
| 314 latitude, longitude, alt_elip, alt_msl, datum, sog, cog, magvar, climbrate, | |
| 315 headrate, estHPE, estVPE, estTE, estHVE, clockbias, CBerr, clockdrift, | |
| 316 CDerr, distance, distanceErr, headErr, numsvs, hdop, addmodeinfo) = \ | |
| 317 struct.unpack(fmt, datastr) | |
| 318 tow = float(tow) / 1e3 | |
| 2 | 319 second = float(second) / 1e3 |
| 0 | 320 latitude = float(latitude) / 1e7 |
| 321 longitude = float(longitude) / 1e7 | |
| 322 alt_elip = float(alt_elip) / 1e2 | |
| 323 alt_msl = float(alt_msl) / 1e2 | |
| 324 sog = float(sog) / 1e2 | |
| 325 cog = float(cog) / 1e2 | |
| 326 climbrate = float(climbrate) / 1e2 | |
| 327 headrate = float(headrate) / 1e2 | |
| 328 estHPE = float(estHPE) / 1e2 | |
| 329 estVPE = float(estVPE) / 1e2 | |
| 330 estTE = float(estTE) / 1e2 | |
| 331 estHVE = float(estHVE) / 1e2 | |
| 332 clockbias = float(clockbias) / 1e2 | |
| 333 CBerr = float(CBerr) / 1e2 | |
| 334 clockdrift = float(clockdrift) / 1e2 | |
| 335 CDerr = float(CDerr) / 1e2 | |
| 336 headErr = float(headErr) / 1e2 | |
| 337 hdop = float(hdop) / 5 | |
| 338 | |
| 2 | 339 utctime = timegm((year, month, day, hour, minute, int(second))) |
| 340 utctime = utctime + (second - int(second)) | |
| 341 print " Fix : %s, Sats : %d, Lat: %.06f, Long: %.06f, Alt: %.02fm" % \ | |
| 342 (fixtype[navtype & 0x7], numsvs, latitude, longitude, alt_msl) | |
| 343 print " Date : %04d/%02d/%02d %02d:%02d:%02d.%03d" % \ | |
| 344 (year, month, day, hour, month, int(second), int((second - int(second)) * 1000)) | |
| 345 print " Epoch time : %.3f, Delta %.3f" % (utctime, abs(time.time() - utctime)) | |
| 346 elif data[0] == 0x34: | |
| 347 fmt = '>BBBBBHHIB' | |
| 348 datastr = reduce(lambda x, y: x + y, map(chr, data[1:struct.calcsize(fmt) + 1])) | |
| 349 (hour, minute, second, day, month, year, offsetint, offsetfrac, status) = \ | |
| 350 struct.unpack(fmt, datastr) | |
| 351 offset = offsetint + float(offsetfrac) / 1e9 | |
| 352 print " PPS : %04d/%02d/%02d %02d:%02d:%02d, Offset : %.9f, Status : 0x%02x" % \ | |
| 353 (year, month, day, hour, minute, second, offset, status) | |
| 0 | 354 elif data[0] == 0xa6: |
| 355 print " Message rate : MID 0x%02x, rate 0x%02x" % (data[2], data[3]) | |
| 356 | |
| 357 def enablemsgs(s): | |
| 358 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 359 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x04, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 360 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x07, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 361 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x09, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 362 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x0d, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 363 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 364 #s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x34, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 365 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x1b, 0x01, 0x00, 0x00, 0x00, 0x00]))) | |
| 366 | |
| 367 | |
| 368 def disablemsgs(s): | |
| 369 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 370 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 371 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 372 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 373 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 374 #s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x29, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 375 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 376 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x34, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 377 s.write(Parser.OrdLsttoStr(Parser.Encap([0xa6, 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00]))) | |
| 378 | |
| 379 | |
| 2 | 380 def timegm(tuple): |
| 381 oldtz = None | |
| 382 if 'TZ' in os.environ: | |
| 383 oldtz = os.environ['TZ'] | |
| 384 os.environ['TZ'] = 'UTC' | |
| 385 | |
| 386 t = time.mktime(tuple + (0, 0, 0)) | |
| 387 if oldtz == None: | |
| 388 del os.environ['TZ'] | |
| 389 else: | |
| 390 os.environ['TZ'] = oldtz | |
| 391 | |
| 392 return t |