diff options
Diffstat (limited to 'scripts/motan/readlog.py')
| -rw-r--r-- | scripts/motan/readlog.py | 311 |
1 files changed, 311 insertions, 0 deletions
diff --git a/scripts/motan/readlog.py b/scripts/motan/readlog.py new file mode 100644 index 00000000..21f4485c --- /dev/null +++ b/scripts/motan/readlog.py @@ -0,0 +1,311 @@ +# Code for reading data logs produced by data_logger.py +# +# Copyright (C) 2021 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import json, zlib + +class error(Exception): + pass + + +###################################################################### +# Log data handlers +###################################################################### + +# Log data handlers: {name: class, ...} +LogHandlers = {} + +# Extract requested position, velocity, and accel from a trapq log +class HandleTrapQ: + ParametersSubscriptionId = 2 + ParametersMin = ParametersMax = 3 + DataSets = [ + ('trapq:<name>:velocity', 'Requested velocity for the given trapq'), + ('trapq:<name>:<axis>', 'Requested axis (x, y, or z) position'), + ('trapq:<name>:<axis>_velocity', 'Requested axis velocity'), + ('trapq:<name>:<axis>_accel', 'Requested axis acceleration'), + ] + def __init__(self, lmanager, name): + self.name = name + self.jdispatch = lmanager.get_jdispatch() + self.cur_data = [(0., 0., 0., 0., (0., 0., 0.), (0., 0., 0.))] + self.data_pos = 0 + tq, trapq_name, datasel = name.split(':') + ptypes = {} + ptypes['velocity'] = { + 'label': '%s velocity' % (trapq_name,), + 'units': 'Velocity\n(mm/s)', 'func': self._pull_velocity + } + ptypes['accel'] = { + 'label': '%s acceleration' % (trapq_name,), + 'units': 'Acceleration\n(mm/s^2)', 'func': self._pull_accel + } + for axis, name in enumerate("xyz"): + ptypes['%s' % (name,)] = { + 'label': '%s %s position' % (trapq_name, name), 'axis': axis, + 'units': 'Position\n(mm)', 'func': self._pull_axis_position + } + ptypes['%s_velocity' % (name,)] = { + 'label': '%s %s velocity' % (trapq_name, name), 'axis': axis, + 'units': 'Velocity\n(mm/s)', 'func': self._pull_axis_velocity + } + ptypes['%s_accel' % (name,)] = { + 'label': '%s %s acceleration' % (trapq_name, name), + 'axis': axis, 'units': 'Acceleration\n(mm/s^2)', + 'func': self._pull_axis_accel + } + pinfo = ptypes.get(datasel) + if pinfo is None: + raise error("Unknown trapq data selection '%s'" % (datasel,)) + self.label = {'label': pinfo['label'], 'units': pinfo['units']} + self.axis = pinfo.get('axis') + self.pull_data = pinfo['func'] + def get_label(self): + return self.label + def _find_move(self, req_time): + data_pos = self.data_pos + while 1: + move = self.cur_data[data_pos] + print_time, move_t, start_v, accel, start_pos, axes_r = move + if req_time <= print_time + move_t: + return move, req_time >= print_time + data_pos += 1 + if data_pos < len(self.cur_data): + self.data_pos = data_pos + continue + jmsg = self.jdispatch.pull_msg(req_time, self.name) + if jmsg is None: + return move, False + self.cur_data = jmsg['data'] + self.data_pos = data_pos = 0 + def _pull_axis_position(self, req_time): + move, in_range = self._find_move(req_time) + print_time, move_t, start_v, accel, start_pos, axes_r = move + mtime = max(0., min(move_t, req_time - print_time)) + dist = (start_v + .5 * accel * mtime) * mtime; + return start_pos[self.axis] + axes_r[self.axis] * dist + def _pull_axis_velocity(self, req_time): + move, in_range = self._find_move(req_time) + if not in_range: + return 0. + print_time, move_t, start_v, accel, start_pos, axes_r = move + return (start_v + accel * (req_time - print_time)) * axes_r[self.axis] + def _pull_axis_accel(self, req_time): + move, in_range = self._find_move(req_time) + if not in_range: + return 0. + print_time, move_t, start_v, accel, start_pos, axes_r = move + return accel * axes_r[self.axis] + def _pull_velocity(self, req_time): + move, in_range = self._find_move(req_time) + if not in_range: + return 0. + print_time, move_t, start_v, accel, start_pos, axes_r = move + return start_v + accel * (req_time - print_time) + def _pull_accel(self, req_time): + move, in_range = self._find_move(req_time) + if not in_range: + return 0. + print_time, move_t, start_v, accel, start_pos, axes_r = move + return accel +LogHandlers["trapq"] = HandleTrapQ + +# Extract positions from queue_step log +class HandleStepQ: + ParametersSubscriptionId = 2 + ParametersMin = ParametersMax = 2 + DataSets = [ + ('stepq:<stepper>', 'Commanded position of the given stepper'), + ] + def __init__(self, lmanager, name): + self.name = name + self.jdispatch = lmanager.get_jdispatch() + self.step_data = [(0., 0.), (0., 0.)] # [(time, pos), ...] + self.data_pos = 0 + def get_label(self): + label = '%s position' % (self.name.split(':')[1],) + return {'label': label, 'units': 'Position\n(mm)'} + def pull_data(self, req_time): + while 1: + data_pos = self.data_pos + step_data = self.step_data + next_time, next_pos = step_data[data_pos + 1] + if req_time < next_time: + return step_data[data_pos][1] + if data_pos + 2 < len(step_data): + self.data_pos = data_pos + 1 + continue + self._pull_block(req_time) + def _pull_block(self, req_time): + step_data = self.step_data + del step_data[:-1] + self.data_pos = 0 + # Read data block containing requested time frame + while 1: + jmsg = self.jdispatch.pull_msg(req_time, self.name) + if jmsg is None: + self.step_data.append((req_time + .1, step_data[0][1])) + return + last_time = jmsg['last_step_time'] + if req_time <= last_time: + break + # Process block into (time, position) 2-tuples + first_time = step_time = jmsg['first_step_time'] + first_clock = jmsg['first_clock'] + step_clock = first_clock - jmsg['data'][0][0] + cdiff = jmsg['last_clock'] - first_clock + tdiff = last_time - first_time + inv_freq = 0. + if cdiff: + inv_freq = tdiff / cdiff + step_dist = jmsg['step_distance'] + step_pos = jmsg['start_position'] + for interval, raw_count, add in jmsg['data']: + qs_dist = step_dist + count = raw_count + if count < 0: + qs_dist = -qs_dist + count = -count + for i in range(count): + step_clock += interval + interval += add + step_time = first_time + (step_clock - first_clock) * inv_freq + step_pos += qs_dist + step_data.append((step_time, step_pos)) +LogHandlers["stepq"] = HandleStepQ + + +###################################################################### +# List datasets +###################################################################### + +def list_datasets(): + datasets = [] + for lh in sorted(LogHandlers.keys()): + datasets += LogHandlers[lh].DataSets + return datasets + + +###################################################################### +# Log reading +###################################################################### + +# Read, uncompress, and parse messages in a log built by data_logger.py +class JsonLogReader: + def __init__(self, filename): + self.file = open(filename, "rb") + self.comp = zlib.decompressobj(31) + self.msgs = [b""] + def seek(self, pos): + self.file.seek(pos) + self.comp = zlib.decompressobj(-15) + def pull_msg(self): + msgs = self.msgs + while 1: + if len(msgs) > 1: + msg = msgs.pop(0) + try: + json_msg = json.loads(msg) + except: + logging.exception("Unable to parse line") + continue + return json_msg + raw_data = self.file.read(8192) + if not raw_data: + return None + data = self.comp.decompress(raw_data) + parts = data.split(b'\x03') + parts[0] = msgs[0] + parts[0] + self.msgs = msgs = parts + +# Store messages in per-subscription queues until handlers are ready for them +class JsonDispatcher: + def __init__(self, log_prefix): + self.names = {} + self.queues = {} + self.last_read_time = 0. + self.log_reader = JsonLogReader(log_prefix + ".json.gz") + self.is_eof = False + def check_end_of_data(self): + return self.is_eof and not any(self.queues.values()) + def add_handler(self, name, subscription_id): + self.names[name] = q = [] + self.queues.setdefault(subscription_id, []).append(q) + def pull_msg(self, req_time, name): + q = self.names[name] + while 1: + if q: + return q.pop(0) + if req_time + 1. < self.last_read_time: + return None + json_msg = self.log_reader.pull_msg() + if json_msg is None: + self.is_eof = True + return None + qid = json_msg.get('q') + if qid == 'status': + pt = json_msg.get('toolhead', {}).get('estimated_print_time') + if pt is not None: + self.last_read_time = pt + for mq in self.queues.get(qid, []): + mq.append(json_msg['params']) + +# Main log access management +class LogManager: + error = error + def __init__(self, log_prefix): + self.index_reader = JsonLogReader(log_prefix + ".index.gz") + self.jdispatch = JsonDispatcher(log_prefix) + self.initial_start_time = self.start_time = 0. + self.datasets = {} + self.initial_status = {} + self.log_subscriptions = {} + self.status = {} + def setup_index(self): + fmsg = self.index_reader.pull_msg() + self.initial_status = status = fmsg['status'] + self.status = dict(status) + start_time = status['toolhead']['estimated_print_time'] + self.initial_start_time = self.start_time = start_time + self.log_subscriptions = fmsg.get('subscriptions', {}) + def get_initial_status(self): + return self.initial_status + def available_dataset_types(self): + return {name: None for name in LogHandlers} + def get_jdispatch(self): + return self.jdispatch + def seek_time(self, req_time): + self.start_time = req_start_time = self.initial_start_time + req_time + seek_time = max(self.initial_start_time, req_start_time - 1.) + file_position = 0 + while 1: + fmsg = self.index_reader.pull_msg() + if fmsg is None: + break + th = fmsg['status']['toolhead'] + ptime = max(th['estimated_print_time'], th.get('print_time', 0.)) + if ptime > seek_time: + break + file_position = fmsg['file_position'] + if file_position: + self.jdispatch.log_reader.seek(file_position) + def get_initial_start_time(self): + return self.initial_start_time + def get_start_time(self): + return self.start_time + def setup_dataset(self, name): + if name in self.datasets: + return self.datasets[name] + parts = name.split(':') + cls = LogHandlers.get(parts[0]) + if cls is None: + raise error("Unknown dataset '%s'" % (parts[0],)) + if len(parts) < cls.ParametersMin or len(parts) > cls.ParametersMax: + raise error("Invalid number of parameters for %s" % (parts[0],)) + subscription_id = ":".join(parts[:cls.ParametersSubscriptionId]) + if subscription_id not in self.log_subscriptions: + raise error("Dataset '%s' not in capture" % (subscription_id,)) + self.datasets[name] = hdl = cls(self, name) + self.jdispatch.add_handler(name, subscription_id) + return hdl |