1# http://pyrocko.org - GPLv3
2#
3# The Pyrocko Developers, 21st Century
4# ---|P------/S----------~Lg----------
5from __future__ import absolute_import, print_function
7import struct
8import collections
9import time
10import logging
11import calendar
12import numpy as num
14from pyrocko import trace, util
16logger = logging.getLogger('pyrocko.streaming.edl')
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)))
29def quotechars(chars):
30 return ''.join(['.', c][c.isalnum()] for c in chars)
33MINBLOCKSIZE = 192
36class NotAquiring(Exception):
37 pass
40class ReadError(Exception):
41 pass
44class ReadTimeout(ReadError):
45 pass
48class ReadUnexpected(ReadError):
49 pass
52class GPSError(Exception):
53 pass
56class NoGPS(GPSError):
57 pass
60class NoGPSTime(GPSError):
61 pass
64class GPSTimeNotUTC(GPSError):
65 pass
68block_def = {}
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()
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()
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()
143block_def['DAT\0'] = '''
144 identifier 4s
145 size_of_dat I
146'''.split()
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)
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'''
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'''
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'''
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'''
209def latlon_float(s):
210 return int(s) / 100000.
213def seconds_float(s):
214 return int(s) / 1000.
217def hex_int(s):
218 return int(s, 16)
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}
231class GPSFormat_(object):
232 def __init__(self, name, fmt):
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
244 self.items = items
245 self.Message = collections.namedtuple('GPSMessage'+k, names)
247 def unpack(self, s):
248 return self.Message(
249 *(converter(s[begin:end])
250 for (begin, end, converter) in self.items))
253GPSFormat = {}
254for k in gps_fmt.keys():
255 GPSFormat[k] = GPSFormat_(k, gps_fmt[k])
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())
264 except Exception:
265 # may only work on some linuxes
266 from glob import glob
267 names = sorted(glob('/dev/ttyS*') + glob('/dev/ttyUSB*'))
269 return names
272def unpack_block(data):
273 block_type = data[:4]
275 if block_type not in Blocks:
276 return None
278 fmt, fmt_len, Block = Blocks[block_type]
280 if len(data) < fmt_len:
281 raise ReadError('block size too short')
283 return Block(*struct.unpack(fmt, data[:fmt_len])), data[fmt_len:]
286def unpack_values(ncomps, bytes_per_sample, data):
287 if bytes_per_sample == 4:
288 return num.fromstring(data, dtype=num.dtype('<i4'))
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)
298 else:
299 raise ReadError('unimplemented bytes_per_sample setting')
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
310 def insert(self, deltat, nadd, t):
312 if self._deltat is None or self._deltat != deltat:
313 self.reset()
314 self._deltat = deltat
316 if self._t0 is None and t is not None:
317 self._t0 = int(round(t/self._deltat))*self._deltat
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
326 self._queue.append((self._n, t))
327 self._n += nadd
329 while len(self._queue) > self._nlookback:
330 self._queue.pop(0)
332 ns, ts = num.array(self._queue, dtype=float).T
334 tpredicts = self._t0 + ns * self._deltat
336 terrors = ts - tpredicts
337 mterror = num.median(terrors)
338 print(mterror / deltat, '+-', num.std(terrors) / deltat)
340 if num.abs(mterror) > 0.75*deltat and \
341 len(self._queue) == self._nlookback:
343 t0 = self._t0 + mterror
344 self._queue[:] = []
345 self._t0 = int(round(t0/self._deltat))*self._deltat
347 return self._t0 + (self._n-nadd)*self._deltat
349 def reset(self):
350 self._queue[:] = []
351 self._n = 0
352 self._t0 = None
354 def __len__(self):
355 return len(self._queue)
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
365 @property
366 def time(self):
367 if not self._st.tracking_status_code == 0:
368 raise NoGPSTime()
370 if not self._tm.gps_utc_offset_flag:
371 raise GPSTimeNotUTC()
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)))
377 @property
378 def latitude(self):
379 return self._pv.latitude
381 @property
382 def longitude(self):
383 return self._pv.longitude
385 @property
386 def altitude(self):
387 return self._al.altitude
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)
398def stime_none_aware(t):
399 if t is None:
400 return '?'
401 else:
402 return util.time_to_str(t)
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
416 def set_approx_times(
417 self, approx_system_time, approx_gps_time, measured_system_time):
419 self._approx_system_time = approx_system_time
420 self._approx_gps_time = approx_gps_time
421 self._measured_system_time = measured_system_time
423 @property
424 def time(self):
425 if self._mod.reserved1 != 0:
426 return float(self._mod.reserved1)
428 return self._approx_system_time
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])
440 traces.append(tr)
442 traces.extend(self.traces_delays())
444 return traces
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)):
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]))
460 traces.append(tr)
462 return traces
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:])
469 @property
470 def gps(self):
471 if self._mod.block_count != self._mod.gps_message_block_count:
472 raise NoGPS()
474 if self._gps is not None:
475 return self._gps
477 kwargs = {}
478 for mess in self._gps_messages():
479 kwargs[mess.type.lower()] = mess
481 if sorted(kwargs.keys()) == ['al', 'pv', 'st', 'tm']:
482 self._gps = GPSRecord(**kwargs)
483 return self._gps
484 else:
485 raise NoGPS()
487 @property
488 def gps_time_or_none(self):
489 try:
490 return self.gps.time
491 except GPSError:
492 return None
494 def __str__(self):
495 return '\n'.join([
496 '%s' % str(x) for x in (self._mod, self._mde)]) + '\n'
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)])
509class Reader(object):
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)
517 self._timeout = float(timeout)
518 self._baudrate = int(baudrate)
519 self._serial = None
520 self._buffer = ''
521 self._irecord = 0
523 self._time_estimator_system = TimeEstimator(lookback)
524 self._time_estimator_gps = TimeEstimator(lookback)
526 def running(self):
527 return self._serial is not None
529 def assert_running(self):
530 if not self.running():
531 raise NotAquiring()
533 def start(self):
534 self.stop()
536 import serial
537 self._serial = serial.Serial(
538 port=self._port,
539 baudrate=self._baudrate,
540 timeout=self._timeout)
542 self._sync_on_mod()
544 def _sync_on_mod(self):
545 self._fill_buffer(MINBLOCKSIZE)
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:]
554 self._fill_buffer(MINBLOCKSIZE)
556 def _fill_buffer(self, minlen):
557 if len(self._buffer) >= minlen:
558 return
560 nread = minlen-len(self._buffer)
561 try:
562 data = self._serial.read(nread)
563 hexdump(data)
565 except Exception:
566 raise ReadError()
568 if len(data) != nread:
569 self.stop()
570 raise ReadTimeout()
571 self._buffer += data
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()
580 block_len += 8
581 self._fill_buffer(block_len)
582 block_data = self._buffer
583 self._buffer = ''
584 return unpack_block(block_data)
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)
599 try:
600 gpstime = r.gps.time
601 except GPSError:
602 gpstime = None
604 approx_gps_time = self._time_estimator_gps.insert(
605 deltat, values.size//mod.ncomps, gpstime)
607 r.set_approx_times(
608 approx_system_time, approx_gps_time, measured_system_time)
610 return r
612 def stop(self):
613 if not self.running():
614 return
616 self._serial.close()
617 self._serial = None
618 self._buffer = ''
619 self._time_estimator_system.reset()
620 self._time_estimator_gps.reset()
623class EDLHamster(object):
624 def __init__(self, *args, **kwargs):
625 self.reader = Reader(*args, **kwargs)
627 def acquisition_start(self):
628 self.reader.start()
630 def acquisition_stop(self):
631 self.reader.stop()
633 def process(self):
634 rec = self.reader.read_record()
635 self.got_record(rec)
637 def got_record(self, rec):
638 for tr in rec.traces:
639 self.got_trace(tr)
641 def got_trace(self, tr):
642 logger.info('Got trace from EDL: %s' % tr)