1# http://pyrocko.org - GPLv3
2#
3# The Pyrocko Developers, 21st Century
4# ---|P------/S----------~Lg----------
6import struct
7import collections
8import time
9import logging
10import calendar
11import numpy as num
13from pyrocko import trace, util
15logger = logging.getLogger('pyrocko.streaming.edl')
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)))
28def quotechars(chars):
29 return ''.join(['.', c][c.isalnum()] for c in chars)
32MINBLOCKSIZE = 192
35class NotAquiring(Exception):
36 pass
39class ReadError(Exception):
40 pass
43class ReadTimeout(ReadError):
44 pass
47class ReadUnexpected(ReadError):
48 pass
51class GPSError(Exception):
52 pass
55class NoGPS(GPSError):
56 pass
59class NoGPSTime(GPSError):
60 pass
63class GPSTimeNotUTC(GPSError):
64 pass
67block_def = {}
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()
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()
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()
142block_def['DAT\0'] = '''
143 identifier 4s
144 size_of_dat I
145'''.split()
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)
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'''
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'''
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'''
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'''
208def latlon_float(s):
209 return int(s) / 100000.
212def seconds_float(s):
213 return int(s) / 1000.
216def hex_int(s):
217 return int(s, 16)
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}
230class GPSFormat_(object):
231 def __init__(self, name, fmt):
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
243 self.items = items
244 self.Message = collections.namedtuple('GPSMessage'+k, names)
246 def unpack(self, s):
247 return self.Message(
248 *(converter(s[begin:end])
249 for (begin, end, converter) in self.items))
252GPSFormat = {}
253for k in gps_fmt.keys():
254 GPSFormat[k] = GPSFormat_(k, gps_fmt[k])
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())
263 except Exception:
264 # may only work on some linuxes
265 from glob import glob
266 names = sorted(glob('/dev/ttyS*') + glob('/dev/ttyUSB*'))
268 return names
271def unpack_block(data):
272 block_type = data[:4]
274 if block_type not in Blocks:
275 return None
277 fmt, fmt_len, Block = Blocks[block_type]
279 if len(data) < fmt_len:
280 raise ReadError('block size too short')
282 return Block(*struct.unpack(fmt, data[:fmt_len])), data[fmt_len:]
285def unpack_values(ncomps, bytes_per_sample, data):
286 if bytes_per_sample == 4:
287 return num.frombuffer(data, dtype=num.dtype('<i4'))
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)
297 else:
298 raise ReadError('unimplemented bytes_per_sample setting')
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
309 def insert(self, deltat, nadd, t):
311 if self._deltat is None or self._deltat != deltat:
312 self.reset()
313 self._deltat = deltat
315 if self._t0 is None and t is not None:
316 self._t0 = int(round(t/self._deltat))*self._deltat
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
325 self._queue.append((self._n, t))
326 self._n += nadd
328 while len(self._queue) > self._nlookback:
329 self._queue.pop(0)
331 ns, ts = num.array(self._queue, dtype=float).T
333 tpredicts = self._t0 + ns * self._deltat
335 terrors = ts - tpredicts
336 mterror = num.median(terrors)
337 print(mterror / deltat, '+-', num.std(terrors) / deltat)
339 if num.abs(mterror) > 0.75*deltat and \
340 len(self._queue) == self._nlookback:
342 t0 = self._t0 + mterror
343 self._queue[:] = []
344 self._t0 = int(round(t0/self._deltat))*self._deltat
346 return self._t0 + (self._n-nadd)*self._deltat
348 def reset(self):
349 self._queue[:] = []
350 self._n = 0
351 self._t0 = None
353 def __len__(self):
354 return len(self._queue)
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
364 @property
365 def time(self):
366 if not self._st.tracking_status_code == 0:
367 raise NoGPSTime()
369 if not self._tm.gps_utc_offset_flag:
370 raise GPSTimeNotUTC()
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)))
376 @property
377 def latitude(self):
378 return self._pv.latitude
380 @property
381 def longitude(self):
382 return self._pv.longitude
384 @property
385 def altitude(self):
386 return self._al.altitude
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)
397def stime_none_aware(t):
398 if t is None:
399 return '?'
400 else:
401 return util.time_to_str(t)
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
415 def set_approx_times(
416 self, approx_system_time, approx_gps_time, measured_system_time):
418 self._approx_system_time = approx_system_time
419 self._approx_gps_time = approx_gps_time
420 self._measured_system_time = measured_system_time
422 @property
423 def time(self):
424 if self._mod.reserved1 != 0:
425 return float(self._mod.reserved1)
427 return self._approx_system_time
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])
439 traces.append(tr)
441 traces.extend(self.traces_delays())
443 return traces
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)):
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]))
459 traces.append(tr)
461 return traces
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:])
468 @property
469 def gps(self):
470 if self._mod.block_count != self._mod.gps_message_block_count:
471 raise NoGPS()
473 if self._gps is not None:
474 return self._gps
476 kwargs = {}
477 for mess in self._gps_messages():
478 kwargs[mess.type.lower()] = mess
480 if sorted(kwargs.keys()) == ['al', 'pv', 'st', 'tm']:
481 self._gps = GPSRecord(**kwargs)
482 return self._gps
483 else:
484 raise NoGPS()
486 @property
487 def gps_time_or_none(self):
488 try:
489 return self.gps.time
490 except GPSError:
491 return None
493 def __str__(self):
494 return '\n'.join([
495 '%s' % str(x) for x in (self._mod, self._mde)]) + '\n'
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)])
508class Reader(object):
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)
516 self._timeout = float(timeout)
517 self._baudrate = int(baudrate)
518 self._serial = None
519 self._buffer = ''
520 self._irecord = 0
522 self._time_estimator_system = TimeEstimator(lookback)
523 self._time_estimator_gps = TimeEstimator(lookback)
525 def running(self):
526 return self._serial is not None
528 def assert_running(self):
529 if not self.running():
530 raise NotAquiring()
532 def start(self):
533 self.stop()
535 import serial
536 self._serial = serial.Serial(
537 port=self._port,
538 baudrate=self._baudrate,
539 timeout=self._timeout)
541 self._sync_on_mod()
543 def _sync_on_mod(self):
544 self._fill_buffer(MINBLOCKSIZE)
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:]
553 self._fill_buffer(MINBLOCKSIZE)
555 def _fill_buffer(self, minlen):
556 if len(self._buffer) >= minlen:
557 return
559 nread = minlen-len(self._buffer)
560 try:
561 data = self._serial.read(nread)
562 hexdump(data)
564 except Exception:
565 raise ReadError()
567 if len(data) != nread:
568 self.stop()
569 raise ReadTimeout()
570 self._buffer += data
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()
579 block_len += 8
580 self._fill_buffer(block_len)
581 block_data = self._buffer
582 self._buffer = ''
583 return unpack_block(block_data)
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)
598 try:
599 gpstime = r.gps.time
600 except GPSError:
601 gpstime = None
603 approx_gps_time = self._time_estimator_gps.insert(
604 deltat, values.size//mod.ncomps, gpstime)
606 r.set_approx_times(
607 approx_system_time, approx_gps_time, measured_system_time)
609 return r
611 def stop(self):
612 if not self.running():
613 return
615 self._serial.close()
616 self._serial = None
617 self._buffer = ''
618 self._time_estimator_system.reset()
619 self._time_estimator_gps.reset()
622class EDLHamster(object):
623 def __init__(self, *args, **kwargs):
624 self.reader = Reader(*args, **kwargs)
626 def acquisition_start(self):
627 self.reader.start()
629 def acquisition_stop(self):
630 self.reader.stop()
632 def process(self):
633 rec = self.reader.read_record()
634 self.got_record(rec)
636 def got_record(self, rec):
637 for tr in rec.traces:
638 self.got_trace(tr)
640 def got_trace(self, tr):
641 logger.info('Got trace from EDL: %s' % tr)