1# http://pyrocko.org - GPLv3 

2# 

3# The Pyrocko Developers, 21st Century 

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

5from __future__ import absolute_import, print_function 

6 

7import struct 

8import collections 

9import time 

10import logging 

11import calendar 

12import numpy as num 

13 

14from pyrocko import trace, util 

15 

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

17 

18 

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

20 while chars: 

21 line = chars[:width] 

22 chars = chars[width:] 

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

24 print("%s%s%s" % ( 

25 sep.join("%02x" % ord(c) for c in line), 

26 sep, quotechars(line))) 

27 

28 

29def quotechars(chars): 

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

31 

32 

33MINBLOCKSIZE = 192 

34 

35 

36class NotAquiring(Exception): 

37 pass 

38 

39 

40class ReadError(Exception): 

41 pass 

42 

43 

44class ReadTimeout(ReadError): 

45 pass 

46 

47 

48class ReadUnexpected(ReadError): 

49 pass 

50 

51 

52class GPSError(Exception): 

53 pass 

54 

55 

56class NoGPS(GPSError): 

57 pass 

58 

59 

60class NoGPSTime(GPSError): 

61 pass 

62 

63 

64class GPSTimeNotUTC(GPSError): 

65 pass 

66 

67 

68block_def = {} 

69 

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

71 identifier 4s 

72 size_of_mod I 

73 device_id 12s 

74 version 6s 

75 space1 1s 

76 serial_no 4s 

77 space2 1s 

78 test_pattern 8s 

79 block_count I 

80 ncomps H 

81 sample_rate H 

82 bytes_per_sample H 

83 filter_type 2s 

84 decimation3 H 

85 decimation4 H 

86 plldata h 

87 gain_g 1s 

88 gain B 

89 gain_component1 I 

90 gain_component2 I 

91 gain_component3 I 

92 offset_component1 I 

93 offset_component2 I 

94 offset_component3 I 

95 supply_voltage H 

96 supply_current H 

97 temp_sensor_voltage H 

98 supply_voltage_remote H 

99 user_input1 H 

100 user_input2 H 

101 user_input3 H 

102 not_used H 

103 coupling 2s 

104 reserved1 I 

105 reserved2 I 

106 reserved3 I 

107 gps_status_flags H 

108 gps_message_block_count I 

109 gps_message 72s 

110'''.split() 

111 

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

113 identifier 4s 

114 size_of_mde I 

115 serial_no 8s 

116 decimation5 H 

117 decimation6 H 

118 decimation7 H 

119 gain_component4 I 

120 gain_component5 I 

121 gain_component6 I 

122 offset_component4 I 

123 offset_component5 I 

124 offset_component6 I 

125 temperature1 H 

126 temperature2 H 

127 temperature3 H 

128 temperature4 H 

129 temperature5 H 

130 temperature6 H 

131 gps_message 129s 

132 pad 1s 

133'''.split() 

134 

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

136 identifier 4s 

137 size_of_sum I 

138 reserved H 

139 checksum_lo B 

140 checksum_hi B 

141'''.split() 

142 

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

144 identifier 4s 

145 size_of_dat I 

146'''.split() 

147 

148 

149Blocks = {} 

150for k in block_def.keys(): 

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

152 fmt_len = struct.calcsize(fmt) 

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

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

155 

156 

157gps_fmt = {} 

158# gps altitude message 

159gps_fmt['AL'] = ''' 

160 type 2 str 

161 time_of_day 5 float 

162 altitude 6 float 

163 vertical_velocity 4 float 

164 source_flag 1 int 

165 age_flag 1 int 

166''' 

167 

168# gps position velocity message 

169gps_fmt['PV'] = ''' 

170 type 2 str 

171 time_of_day 5 float 

172 latitude 8 lat_float 

173 longitude 9 lon_float 

174 speed_mph 3 float 

175 heading 3 float 

176 source_flag 1 int 

177 age_flag 1 int 

178''' 

179 

180# gps status message 

181gps_fmt['ST'] = ''' 

182 type 2 str 

183 tracking_status_code 2 hex_int 

184 nibble1 1 hex_int 

185 nibble2 1 hex_int 

186 machine_id 2 hex_int 

187 nibble3 1 hex_int 

188 nibble4 1 hex_int 

189 reserved 2 str 

190''' 

191 

192# gps time date message 

193gps_fmt['TM'] = ''' 

194 type 2 str 

195 hours 2 int 

196 minutes 2 int 

197 seconds 5 seconds_float 

198 day 2 int 

199 month 2 int 

200 year 4 int 

201 gps_utc_time_offset 2 int 

202 current_fix_source 1 int 

203 number_usable_svs 2 int 

204 gps_utc_offset_flag 1 int 

205 reserved 5 str 

206''' 

207 

208 

209def latlon_float(s): 

210 return int(s) / 100000. 

211 

212 

213def seconds_float(s): 

214 return int(s) / 1000. 

215 

216 

217def hex_int(s): 

218 return int(s, 16) 

219 

220 

221convert_functions = { 

222 'int': int, 

223 'float': float, 

224 'lat_float': latlon_float, 

225 'lon_float': latlon_float, 

226 'seconds_float': seconds_float, 

227 'str': str, 

228 'hex_int': hex_int} 

229 

230 

231class GPSFormat_(object): 

232 def __init__(self, name, fmt): 

233 

234 nn = 0 

235 names = [] 

236 items = [] 

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

238 toks = line.split() 

239 n = int(toks[1]) 

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

241 names.append(toks[0]) 

242 nn += n 

243 

244 self.items = items 

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

246 

247 def unpack(self, s): 

248 return self.Message( 

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

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

251 

252 

253GPSFormat = {} 

254for k in gps_fmt.keys(): 

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

256 

257 

258def portnames(): 

259 try: 

260 # needs serial >= v2.6 

261 from serial.tools.list_ports import comports 

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

263 

264 except Exception: 

265 # may only work on some linuxes 

266 from glob import glob 

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

268 

269 return names 

270 

271 

272def unpack_block(data): 

273 block_type = data[:4] 

274 

275 if block_type not in Blocks: 

276 return None 

277 

278 fmt, fmt_len, Block = Blocks[block_type] 

279 

280 if len(data) < fmt_len: 

281 raise ReadError('block size too short') 

282 

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

284 

285 

286def unpack_values(ncomps, bytes_per_sample, data): 

287 if bytes_per_sample == 4: 

288 return num.fromstring(data, dtype=num.dtype('<i4')) 

289 

290 # 3-byte mode is broken: 

291 # elif bytes_per_sample == 3: 

292 # b1 = num.fromstring(data, dtype=num.dtype('<i1')) 

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

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

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

296 # return b4.astype(num.int32) 

297 

298 else: 

299 raise ReadError('unimplemented bytes_per_sample setting') 

300 

301 

302class TimeEstimator(object): 

303 def __init__(self, nlookback): 

304 self._nlookback = nlookback 

305 self._queue = [] 

306 self._t0 = None 

307 self._n = 0 

308 self._deltat = None 

309 

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

311 

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

313 self.reset() 

314 self._deltat = deltat 

315 

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

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

318 

319 if t is None: 

320 self._n += nadd 

321 if self._t0: 

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

323 else: 

324 return None 

325 

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

327 self._n += nadd 

328 

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

330 self._queue.pop(0) 

331 

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

333 

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

335 

336 terrors = ts - tpredicts 

337 mterror = num.median(terrors) 

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

339 

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

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

342 

343 t0 = self._t0 + mterror 

344 self._queue[:] = [] 

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

346 

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

348 

349 def reset(self): 

350 self._queue[:] = [] 

351 self._n = 0 

352 self._t0 = None 

353 

354 def __len__(self): 

355 return len(self._queue) 

356 

357 

358class GPSRecord(object): 

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

360 self._al = al 

361 self._pv = pv 

362 self._st = st 

363 self._tm = tm 

364 

365 @property 

366 def time(self): 

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

368 raise NoGPSTime() 

369 

370 if not self._tm.gps_utc_offset_flag: 

371 raise GPSTimeNotUTC() 

372 

373 tm = self._tm 

374 return util.to_time_float(calendar.timegm(( 

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

376 

377 @property 

378 def latitude(self): 

379 return self._pv.latitude 

380 

381 @property 

382 def longitude(self): 

383 return self._pv.longitude 

384 

385 @property 

386 def altitude(self): 

387 return self._al.altitude 

388 

389 def __str__(self): 

390 try: 

391 stime = util.time_to_str(self.time) 

392 except GPSError: 

393 stime = '?' 

394 return '''%s %s %s %s''' % ( 

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

396 

397 

398def stime_none_aware(t): 

399 if t is None: 

400 return '?' 

401 else: 

402 return util.time_to_str(t) 

403 

404 

405class Record(object): 

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

407 self._mod = mod 

408 self._mde = mde 

409 self._dat = dat 

410 self._sum = sum 

411 self._values = values 

412 self._approx_system_time = None 

413 self._approx_gps_time = None 

414 self._gps = None 

415 

416 def set_approx_times( 

417 self, approx_system_time, approx_gps_time, measured_system_time): 

418 

419 self._approx_system_time = approx_system_time 

420 self._approx_gps_time = approx_gps_time 

421 self._measured_system_time = measured_system_time 

422 

423 @property 

424 def time(self): 

425 if self._mod.reserved1 != 0: 

426 return float(self._mod.reserved1) 

427 

428 return self._approx_system_time 

429 

430 @property 

431 def traces(self): 

432 traces = [] 

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

434 tr = trace.Trace( 

435 '', 'ed', '', 'p%i' % i, 

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

437 tmin=self.time, 

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

439 

440 traces.append(tr) 

441 

442 traces.extend(self.traces_delays()) 

443 

444 return traces 

445 

446 def traces_delays(self): 

447 traces = [] 

448 for name, val in ( 

449 ('gp', self.gps_time_or_none), 

450 ('sm', self._measured_system_time), 

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

452 

453 if val is not None: 

454 tr = trace.Trace( 

455 '', 'ed', name, 'del', 

456 deltat=1.0, 

457 tmin=self.time, 

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

459 

460 traces.append(tr) 

461 

462 return traces 

463 

464 def _gps_messages(self): 

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

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

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

468 

469 @property 

470 def gps(self): 

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

472 raise NoGPS() 

473 

474 if self._gps is not None: 

475 return self._gps 

476 

477 kwargs = {} 

478 for mess in self._gps_messages(): 

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

480 

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

482 self._gps = GPSRecord(**kwargs) 

483 return self._gps 

484 else: 

485 raise NoGPS() 

486 

487 @property 

488 def gps_time_or_none(self): 

489 try: 

490 return self.gps.time 

491 except GPSError: 

492 return None 

493 

494 def __str__(self): 

495 return '\n'.join([ 

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

497 

498 def str_times(self): 

499 return '''--- Record --- 

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

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

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

503 self._approx_gps_time, 

504 self.gps_time_or_none, 

505 self._approx_system_time, 

506 self._measured_system_time)]) 

507 

508 

509class Reader(object): 

510 

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

512 if isinstance(port, int): 

513 self._port = portnames()[port] 

514 else: 

515 self._port = str(port) 

516 

517 self._timeout = float(timeout) 

518 self._baudrate = int(baudrate) 

519 self._serial = None 

520 self._buffer = '' 

521 self._irecord = 0 

522 

523 self._time_estimator_system = TimeEstimator(lookback) 

524 self._time_estimator_gps = TimeEstimator(lookback) 

525 

526 def running(self): 

527 return self._serial is not None 

528 

529 def assert_running(self): 

530 if not self.running(): 

531 raise NotAquiring() 

532 

533 def start(self): 

534 self.stop() 

535 

536 import serial 

537 self._serial = serial.Serial( 

538 port=self._port, 

539 baudrate=self._baudrate, 

540 timeout=self._timeout) 

541 

542 self._sync_on_mod() 

543 

544 def _sync_on_mod(self): 

545 self._fill_buffer(MINBLOCKSIZE) 

546 

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

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

549 if imark != -1: 

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

551 else: 

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

553 

554 self._fill_buffer(MINBLOCKSIZE) 

555 

556 def _fill_buffer(self, minlen): 

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

558 return 

559 

560 nread = minlen-len(self._buffer) 

561 try: 

562 data = self._serial.read(nread) 

563 hexdump(data) 

564 

565 except Exception: 

566 raise ReadError() 

567 

568 if len(data) != nread: 

569 self.stop() 

570 raise ReadTimeout() 

571 self._buffer += data 

572 

573 def _read_block(self, need_block_type=None): 

574 self.assert_running() 

575 self._fill_buffer(8) 

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

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

578 raise ReadUnexpected() 

579 

580 block_len += 8 

581 self._fill_buffer(block_len) 

582 block_data = self._buffer 

583 self._buffer = '' 

584 return unpack_block(block_data) 

585 

586 def read_record(self): 

587 self._irecord += 1 

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

589 measured_system_time = time.time() - 4.0 

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

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

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

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

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

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

596 approx_system_time = self._time_estimator_system.insert( 

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

598 

599 try: 

600 gpstime = r.gps.time 

601 except GPSError: 

602 gpstime = None 

603 

604 approx_gps_time = self._time_estimator_gps.insert( 

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

606 

607 r.set_approx_times( 

608 approx_system_time, approx_gps_time, measured_system_time) 

609 

610 return r 

611 

612 def stop(self): 

613 if not self.running(): 

614 return 

615 

616 self._serial.close() 

617 self._serial = None 

618 self._buffer = '' 

619 self._time_estimator_system.reset() 

620 self._time_estimator_gps.reset() 

621 

622 

623class EDLHamster(object): 

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

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

626 

627 def acquisition_start(self): 

628 self.reader.start() 

629 

630 def acquisition_stop(self): 

631 self.reader.stop() 

632 

633 def process(self): 

634 rec = self.reader.read_record() 

635 self.got_record(rec) 

636 

637 def got_record(self, rec): 

638 for tr in rec.traces: 

639 self.got_trace(tr) 

640 

641 def got_trace(self, tr): 

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