1# http://pyrocko.org - GPLv3 

2# 

3# The Pyrocko Developers, 21st Century 

4# ---|P------/S----------~Lg---------- 

5 

6import struct 

7import collections 

8import time 

9import logging 

10import calendar 

11import numpy as num 

12 

13from pyrocko import trace, util 

14 

15logger = logging.getLogger('pyrocko.streaming.edl') 

16 

17 

18def hexdump(chars, sep=' ', width=16): 

19 while chars: 

20 line = chars[:width] 

21 chars = chars[width:] 

22 line = line.ljust(width, '\000') 

23 print('%s%s%s' % ( 

24 sep.join('%02x' % ord(c) for c in line), 

25 sep, quotechars(line))) 

26 

27 

28def quotechars(chars): 

29 return ''.join(['.', c][c.isalnum()] for c in chars) 

30 

31 

32MINBLOCKSIZE = 192 

33 

34 

35class NotAquiring(Exception): 

36 pass 

37 

38 

39class ReadError(Exception): 

40 pass 

41 

42 

43class ReadTimeout(ReadError): 

44 pass 

45 

46 

47class ReadUnexpected(ReadError): 

48 pass 

49 

50 

51class GPSError(Exception): 

52 pass 

53 

54 

55class NoGPS(GPSError): 

56 pass 

57 

58 

59class NoGPSTime(GPSError): 

60 pass 

61 

62 

63class GPSTimeNotUTC(GPSError): 

64 pass 

65 

66 

67block_def = {} 

68 

69block_def['MOD\0'] = ''' 

70 identifier 4s 

71 size_of_mod I 

72 device_id 12s 

73 version 6s 

74 space1 1s 

75 serial_no 4s 

76 space2 1s 

77 test_pattern 8s 

78 block_count I 

79 ncomps H 

80 sample_rate H 

81 bytes_per_sample H 

82 filter_type 2s 

83 decimation3 H 

84 decimation4 H 

85 plldata h 

86 gain_g 1s 

87 gain B 

88 gain_component1 I 

89 gain_component2 I 

90 gain_component3 I 

91 offset_component1 I 

92 offset_component2 I 

93 offset_component3 I 

94 supply_voltage H 

95 supply_current H 

96 temp_sensor_voltage H 

97 supply_voltage_remote H 

98 user_input1 H 

99 user_input2 H 

100 user_input3 H 

101 not_used H 

102 coupling 2s 

103 reserved1 I 

104 reserved2 I 

105 reserved3 I 

106 gps_status_flags H 

107 gps_message_block_count I 

108 gps_message 72s 

109'''.split() 

110 

111block_def['MDE\0'] = ''' 

112 identifier 4s 

113 size_of_mde I 

114 serial_no 8s 

115 decimation5 H 

116 decimation6 H 

117 decimation7 H 

118 gain_component4 I 

119 gain_component5 I 

120 gain_component6 I 

121 offset_component4 I 

122 offset_component5 I 

123 offset_component6 I 

124 temperature1 H 

125 temperature2 H 

126 temperature3 H 

127 temperature4 H 

128 temperature5 H 

129 temperature6 H 

130 gps_message 129s 

131 pad 1s 

132'''.split() 

133 

134block_def['SUM\0'] = ''' 

135 identifier 4s 

136 size_of_sum I 

137 reserved H 

138 checksum_lo B 

139 checksum_hi B 

140'''.split() 

141 

142block_def['DAT\0'] = ''' 

143 identifier 4s 

144 size_of_dat I 

145'''.split() 

146 

147 

148Blocks = {} 

149for k in block_def.keys(): 

150 fmt = '<'+''.join(block_def[k][1::2]) 

151 fmt_len = struct.calcsize(fmt) 

152 Block = collections.namedtuple('Block'+k[:3], block_def[k][::2]) 

153 Blocks[k] = (fmt, fmt_len, Block) 

154 

155 

156gps_fmt = {} 

157# gps altitude message 

158gps_fmt['AL'] = ''' 

159 type 2 str 

160 time_of_day 5 float 

161 altitude 6 float 

162 vertical_velocity 4 float 

163 source_flag 1 int 

164 age_flag 1 int 

165''' 

166 

167# gps position velocity message 

168gps_fmt['PV'] = ''' 

169 type 2 str 

170 time_of_day 5 float 

171 latitude 8 lat_float 

172 longitude 9 lon_float 

173 speed_mph 3 float 

174 heading 3 float 

175 source_flag 1 int 

176 age_flag 1 int 

177''' 

178 

179# gps status message 

180gps_fmt['ST'] = ''' 

181 type 2 str 

182 tracking_status_code 2 hex_int 

183 nibble1 1 hex_int 

184 nibble2 1 hex_int 

185 machine_id 2 hex_int 

186 nibble3 1 hex_int 

187 nibble4 1 hex_int 

188 reserved 2 str 

189''' 

190 

191# gps time date message 

192gps_fmt['TM'] = ''' 

193 type 2 str 

194 hours 2 int 

195 minutes 2 int 

196 seconds 5 seconds_float 

197 day 2 int 

198 month 2 int 

199 year 4 int 

200 gps_utc_time_offset 2 int 

201 current_fix_source 1 int 

202 number_usable_svs 2 int 

203 gps_utc_offset_flag 1 int 

204 reserved 5 str 

205''' 

206 

207 

208def latlon_float(s): 

209 return int(s) / 100000. 

210 

211 

212def seconds_float(s): 

213 return int(s) / 1000. 

214 

215 

216def hex_int(s): 

217 return int(s, 16) 

218 

219 

220convert_functions = { 

221 'int': int, 

222 'float': float, 

223 'lat_float': latlon_float, 

224 'lon_float': latlon_float, 

225 'seconds_float': seconds_float, 

226 'str': str, 

227 'hex_int': hex_int} 

228 

229 

230class GPSFormat_(object): 

231 def __init__(self, name, fmt): 

232 

233 nn = 0 

234 names = [] 

235 items = [] 

236 for line in fmt.strip().splitlines(): 

237 toks = line.split() 

238 n = int(toks[1]) 

239 items.append((nn, nn+n, convert_functions[toks[2]])) 

240 names.append(toks[0]) 

241 nn += n 

242 

243 self.items = items 

244 self.Message = collections.namedtuple('GPSMessage'+k, names) 

245 

246 def unpack(self, s): 

247 return self.Message( 

248 *(converter(s[begin:end]) 

249 for (begin, end, converter) in self.items)) 

250 

251 

252GPSFormat = {} 

253for k in gps_fmt.keys(): 

254 GPSFormat[k] = GPSFormat_(k, gps_fmt[k]) 

255 

256 

257def portnames(): 

258 try: 

259 # needs serial >= v2.6 

260 from serial.tools.list_ports import comports 

261 names = sorted(x[0] for x in comports()) 

262 

263 except Exception: 

264 # may only work on some linuxes 

265 from glob import glob 

266 names = sorted(glob('/dev/ttyS*') + glob('/dev/ttyUSB*')) 

267 

268 return names 

269 

270 

271def unpack_block(data): 

272 block_type = data[:4] 

273 

274 if block_type not in Blocks: 

275 return None 

276 

277 fmt, fmt_len, Block = Blocks[block_type] 

278 

279 if len(data) < fmt_len: 

280 raise ReadError('block size too short') 

281 

282 return Block(*struct.unpack(fmt, data[:fmt_len])), data[fmt_len:] 

283 

284 

285def unpack_values(ncomps, bytes_per_sample, data): 

286 if bytes_per_sample == 4: 

287 return num.frombuffer(data, dtype=num.dtype('<i4')) 

288 

289 # 3-byte mode is broken: 

290 # elif bytes_per_sample == 3: 

291 # b1 = num.frombuffer(data, dtype=num.dtype('<i1')) 

292 # b4 = num.zeros(len(data)/4, dtype=num.dtype('<i4')) 

293 # b4.view(dtype='<i2')[::2] = b1.view(dtype='<i2') 

294 # b4.view(dtype='<i1')[2::4] = b1[i::3] 

295 # return b4.astype(num.int32) 

296 

297 else: 

298 raise ReadError('unimplemented bytes_per_sample setting') 

299 

300 

301class TimeEstimator(object): 

302 def __init__(self, nlookback): 

303 self._nlookback = nlookback 

304 self._queue = [] 

305 self._t0 = None 

306 self._n = 0 

307 self._deltat = None 

308 

309 def insert(self, deltat, nadd, t): 

310 

311 if self._deltat is None or self._deltat != deltat: 

312 self.reset() 

313 self._deltat = deltat 

314 

315 if self._t0 is None and t is not None: 

316 self._t0 = int(round(t/self._deltat))*self._deltat 

317 

318 if t is None: 

319 self._n += nadd 

320 if self._t0: 

321 return self._t0 + (self._n-nadd)*self._deltat 

322 else: 

323 return None 

324 

325 self._queue.append((self._n, t)) 

326 self._n += nadd 

327 

328 while len(self._queue) > self._nlookback: 

329 self._queue.pop(0) 

330 

331 ns, ts = num.array(self._queue, dtype=float).T 

332 

333 tpredicts = self._t0 + ns * self._deltat 

334 

335 terrors = ts - tpredicts 

336 mterror = num.median(terrors) 

337 print(mterror / deltat, '+-', num.std(terrors) / deltat) 

338 

339 if num.abs(mterror) > 0.75*deltat and \ 

340 len(self._queue) == self._nlookback: 

341 

342 t0 = self._t0 + mterror 

343 self._queue[:] = [] 

344 self._t0 = int(round(t0/self._deltat))*self._deltat 

345 

346 return self._t0 + (self._n-nadd)*self._deltat 

347 

348 def reset(self): 

349 self._queue[:] = [] 

350 self._n = 0 

351 self._t0 = None 

352 

353 def __len__(self): 

354 return len(self._queue) 

355 

356 

357class GPSRecord(object): 

358 def __init__(self, al, pv, st, tm): 

359 self._al = al 

360 self._pv = pv 

361 self._st = st 

362 self._tm = tm 

363 

364 @property 

365 def time(self): 

366 if not self._st.tracking_status_code == 0: 

367 raise NoGPSTime() 

368 

369 if not self._tm.gps_utc_offset_flag: 

370 raise GPSTimeNotUTC() 

371 

372 tm = self._tm 

373 return util.to_time_float(calendar.timegm(( 

374 tm.year, tm.month, tm.day, tm.hours, tm.minutes, tm.seconds))) 

375 

376 @property 

377 def latitude(self): 

378 return self._pv.latitude 

379 

380 @property 

381 def longitude(self): 

382 return self._pv.longitude 

383 

384 @property 

385 def altitude(self): 

386 return self._al.altitude 

387 

388 def __str__(self): 

389 try: 

390 stime = util.time_to_str(self.time) 

391 except GPSError: 

392 stime = '?' 

393 return '''%s %s %s %s''' % ( 

394 stime, self.latitude, self.longitude, self.altitude) 

395 

396 

397def stime_none_aware(t): 

398 if t is None: 

399 return '?' 

400 else: 

401 return util.time_to_str(t) 

402 

403 

404class Record(object): 

405 def __init__(self, mod, mde, dat, sum, values): 

406 self._mod = mod 

407 self._mde = mde 

408 self._dat = dat 

409 self._sum = sum 

410 self._values = values 

411 self._approx_system_time = None 

412 self._approx_gps_time = None 

413 self._gps = None 

414 

415 def set_approx_times( 

416 self, approx_system_time, approx_gps_time, measured_system_time): 

417 

418 self._approx_system_time = approx_system_time 

419 self._approx_gps_time = approx_gps_time 

420 self._measured_system_time = measured_system_time 

421 

422 @property 

423 def time(self): 

424 if self._mod.reserved1 != 0: 

425 return float(self._mod.reserved1) 

426 

427 return self._approx_system_time 

428 

429 @property 

430 def traces(self): 

431 traces = [] 

432 for i in range(self._mod.ncomps): 

433 tr = trace.Trace( 

434 '', 'ed', '', 'p%i' % i, 

435 deltat=float(self._mod.ncomps)/self._mod.sample_rate, 

436 tmin=self.time, 

437 ydata=self._values[i::3]) 

438 

439 traces.append(tr) 

440 

441 traces.extend(self.traces_delays()) 

442 

443 return traces 

444 

445 def traces_delays(self): 

446 traces = [] 

447 for name, val in ( 

448 ('gp', self.gps_time_or_none), 

449 ('sm', self._measured_system_time), 

450 ('sp', self._approx_system_time)): 

451 

452 if val is not None: 

453 tr = trace.Trace( 

454 '', 'ed', name, 'del', 

455 deltat=1.0, 

456 tmin=self.time, 

457 ydata=num.array([val - self.time])) 

458 

459 traces.append(tr) 

460 

461 return traces 

462 

463 def _gps_messages(self): 

464 for line in self._mde.gps_message.splitlines(): 

465 if len(line) > 4 and line[0] == '>' and line.rstrip()[-1] == '<': 

466 yield GPSFormat[line[2:4]].unpack(line[2:]) 

467 

468 @property 

469 def gps(self): 

470 if self._mod.block_count != self._mod.gps_message_block_count: 

471 raise NoGPS() 

472 

473 if self._gps is not None: 

474 return self._gps 

475 

476 kwargs = {} 

477 for mess in self._gps_messages(): 

478 kwargs[mess.type.lower()] = mess 

479 

480 if sorted(kwargs.keys()) == ['al', 'pv', 'st', 'tm']: 

481 self._gps = GPSRecord(**kwargs) 

482 return self._gps 

483 else: 

484 raise NoGPS() 

485 

486 @property 

487 def gps_time_or_none(self): 

488 try: 

489 return self.gps.time 

490 except GPSError: 

491 return None 

492 

493 def __str__(self): 

494 return '\n'.join([ 

495 '%s' % str(x) for x in (self._mod, self._mde)]) + '\n' 

496 

497 def str_times(self): 

498 return '''--- Record --- 

499Time GPS: %s (estimated) %s (measured) 

500Time system: %s (estimated) %s (measured) 

501''' % tuple([stime_none_aware(t) for t in ( 

502 self._approx_gps_time, 

503 self.gps_time_or_none, 

504 self._approx_system_time, 

505 self._measured_system_time)]) 

506 

507 

508class Reader(object): 

509 

510 def __init__(self, port=0, timeout=2., baudrate=115200, lookback=30): 

511 if isinstance(port, int): 

512 self._port = portnames()[port] 

513 else: 

514 self._port = str(port) 

515 

516 self._timeout = float(timeout) 

517 self._baudrate = int(baudrate) 

518 self._serial = None 

519 self._buffer = '' 

520 self._irecord = 0 

521 

522 self._time_estimator_system = TimeEstimator(lookback) 

523 self._time_estimator_gps = TimeEstimator(lookback) 

524 

525 def running(self): 

526 return self._serial is not None 

527 

528 def assert_running(self): 

529 if not self.running(): 

530 raise NotAquiring() 

531 

532 def start(self): 

533 self.stop() 

534 

535 import serial 

536 self._serial = serial.Serial( 

537 port=self._port, 

538 baudrate=self._baudrate, 

539 timeout=self._timeout) 

540 

541 self._sync_on_mod() 

542 

543 def _sync_on_mod(self): 

544 self._fill_buffer(MINBLOCKSIZE) 

545 

546 while self._buffer[:4] != 'MOD\0': 

547 imark = self._buffer.find('MOD\0') 

548 if imark != -1: 

549 self._buffer = self._buffer[imark:] 

550 else: 

551 self._buffer = self._buffer[-4:] 

552 

553 self._fill_buffer(MINBLOCKSIZE) 

554 

555 def _fill_buffer(self, minlen): 

556 if len(self._buffer) >= minlen: 

557 return 

558 

559 nread = minlen-len(self._buffer) 

560 try: 

561 data = self._serial.read(nread) 

562 hexdump(data) 

563 

564 except Exception: 

565 raise ReadError() 

566 

567 if len(data) != nread: 

568 self.stop() 

569 raise ReadTimeout() 

570 self._buffer += data 

571 

572 def _read_block(self, need_block_type=None): 

573 self.assert_running() 

574 self._fill_buffer(8) 

575 block_type, block_len = struct.unpack('<4sI', self._buffer[:8]) 

576 if need_block_type is not None and block_type != need_block_type: 

577 raise ReadUnexpected() 

578 

579 block_len += 8 

580 self._fill_buffer(block_len) 

581 block_data = self._buffer 

582 self._buffer = '' 

583 return unpack_block(block_data) 

584 

585 def read_record(self): 

586 self._irecord += 1 

587 mod, _ = self._read_block('MOD\0') 

588 measured_system_time = time.time() - 4.0 

589 mde, _ = self._read_block('MDE\0') 

590 dat, values_data = self._read_block('DAT\0') 

591 sum, _ = self._read_block('SUM\0') 

592 values = unpack_values(mod.ncomps, mod.bytes_per_sample, values_data) 

593 deltat = 1./mod.sample_rate * mod.ncomps 

594 r = Record(mod, mde, dat, sum, values) 

595 approx_system_time = self._time_estimator_system.insert( 

596 deltat, values.size//mod.ncomps, measured_system_time) 

597 

598 try: 

599 gpstime = r.gps.time 

600 except GPSError: 

601 gpstime = None 

602 

603 approx_gps_time = self._time_estimator_gps.insert( 

604 deltat, values.size//mod.ncomps, gpstime) 

605 

606 r.set_approx_times( 

607 approx_system_time, approx_gps_time, measured_system_time) 

608 

609 return r 

610 

611 def stop(self): 

612 if not self.running(): 

613 return 

614 

615 self._serial.close() 

616 self._serial = None 

617 self._buffer = '' 

618 self._time_estimator_system.reset() 

619 self._time_estimator_gps.reset() 

620 

621 

622class EDLHamster(object): 

623 def __init__(self, *args, **kwargs): 

624 self.reader = Reader(*args, **kwargs) 

625 

626 def acquisition_start(self): 

627 self.reader.start() 

628 

629 def acquisition_stop(self): 

630 self.reader.stop() 

631 

632 def process(self): 

633 rec = self.reader.read_record() 

634 self.got_record(rec) 

635 

636 def got_record(self, rec): 

637 for tr in rec.traces: 

638 self.got_trace(tr) 

639 

640 def got_trace(self, tr): 

641 logger.info('Got trace from EDL: %s' % tr)