diff options
Diffstat (limited to 'klippy')
| -rw-r--r-- | klippy/extras/adxl345.py | 203 | ||||
| -rw-r--r-- | klippy/extras/resonance_tester.py | 2 |
2 files changed, 125 insertions, 80 deletions
diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py index 1c283862..7e21866d 100644 --- a/klippy/extras/adxl345.py +++ b/klippy/extras/adxl345.py @@ -30,53 +30,26 @@ Accel_Measurement = collections.namedtuple( # Helper class to obtain measurements class ADXL345QueryHelper: - def __init__(self, printer, chip, cconn): + def __init__(self, printer, cconn): self.printer = printer - self.chip = chip self.cconn = cconn print_time = printer.lookup_object('toolhead').get_last_move_time() self.request_start_time = self.request_end_time = print_time - self.raw_samples = None self.samples = [] - self.drops = self.overflows = 0 - self.start2_time = 0. - self.time_per_sample = self.start_range = self.end_range = 0. def finish_measurements(self): toolhead = self.printer.lookup_object('toolhead') self.request_end_time = toolhead.get_last_move_time() toolhead.wait_moves() self.cconn.finalize() - toolhead.dwell(0.200) - toolhead.wait_moves() - self._setup_data(*self.chip.final_results) # XXX - def get_stats(self): - return ("drops=%d,overflows=%d" - ",time_per_sample=%.9f,start_range=%.6f,end_range=%.6f" - % (self.drops, self.overflows, - self.time_per_sample, self.start_range, self.end_range)) - def _setup_data(self, end_sequence, overflows, - start1_time, start2_time, end1_time, end2_time): - raw_samples = self.cconn.get_messages() - if not raw_samples or not end_sequence: - return - self.raw_samples = raw_samples - self.overflows = overflows - self.start2_time = start2_time - self.start_range = start2_time - start1_time - self.end_range = end2_time - end1_time - self.total_count = raw_samples[-1]['params']['data'][-1][0] + 1 - total_time = end2_time - start2_time - self.time_per_sample = total_time / self.total_count - actual_count = sum([len(m['params']['data']) for m in raw_samples]) - self.drops = self.total_count - actual_count def decode_samples(self): - if not self.raw_samples: + raw_samples = self.cconn.get_messages() + if not raw_samples: return self.samples + total = sum([len(m['params']['data']) for m in raw_samples]) count = 0 - self.samples = samples = [None] * self.total_count - for msg in self.raw_samples: - for seq, x, y, z in msg['params']['data']: - samp_time = self.start2_time + seq * self.time_per_sample + self.samples = samples = [None] * total + for msg in raw_samples: + for samp_time, x, y, z in msg['params']['data']: if samp_time < self.request_start_time: continue if samp_time > self.request_end_time: @@ -93,8 +66,7 @@ class ADXL345QueryHelper: except: pass f = open(filename, "w") - f.write("##%s\n#time,accel_x,accel_y,accel_z\n" % ( - self.get_stats(),)) + f.write("#time,accel_x,accel_y,accel_z\n") samples = self.samples or self.decode_samples() for t, accel_x, accel_y, accel_z in samples: f.write("%.6f,%.6f,%.6f,%.6f\n" % ( @@ -183,6 +155,54 @@ class ADXLCommandHelper: val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0)) self.chip.set_reg(reg, val) +# Helper class for chip clock synchronization via linear regression +class ClockSyncRegression: + def __init__(self, mcu, chip_clock_smooth, decay = 1. / 20.): + self.mcu = mcu + self.chip_clock_smooth = chip_clock_smooth + self.decay = decay + self.last_chip_clock = self.last_exp_mcu_clock = 0. + self.mcu_clock_avg = self.mcu_clock_variance = 0. + self.chip_clock_avg = self.chip_clock_covariance = 0. + def reset(self, mcu_clock, chip_clock): + self.mcu_clock_avg = self.last_mcu_clock = mcu_clock + self.chip_clock_avg = chip_clock + self.mcu_clock_variance = self.chip_clock_covariance = 0. + self.last_chip_clock = self.last_exp_mcu_clock = 0. + def update(self, mcu_clock, chip_clock): + # Update linear regression + decay = self.decay + diff_mcu_clock = mcu_clock - self.mcu_clock_avg + self.mcu_clock_avg += decay * diff_mcu_clock + self.mcu_clock_variance = (1. - decay) * ( + self.mcu_clock_variance + diff_mcu_clock**2 * decay) + diff_chip_clock = chip_clock - self.chip_clock_avg + self.chip_clock_avg += decay * diff_chip_clock + self.chip_clock_covariance = (1. - decay) * ( + self.chip_clock_covariance + diff_mcu_clock*diff_chip_clock*decay) + def set_last_chip_clock(self, chip_clock): + base_mcu, base_chip, inv_cfreq = self.get_clock_translation() + self.last_chip_clock = chip_clock + self.last_exp_mcu_clock = base_mcu + (chip_clock-base_chip) * inv_cfreq + def get_clock_translation(self): + inv_chip_freq = self.mcu_clock_variance / self.chip_clock_covariance + if not self.last_chip_clock: + return self.mcu_clock_avg, self.chip_clock_avg, inv_chip_freq + # Find mcu clock associated with future chip_clock + s_chip_clock = self.last_chip_clock + self.chip_clock_smooth + scdiff = s_chip_clock - self.chip_clock_avg + s_mcu_clock = self.mcu_clock_avg + scdiff * inv_chip_freq + # Calculate frequency to converge at future point + mdiff = s_mcu_clock - self.last_exp_mcu_clock + s_inv_chip_freq = mdiff / self.chip_clock_smooth + return self.last_exp_mcu_clock, self.last_chip_clock, s_inv_chip_freq + def get_time_translation(self): + base_mcu, base_chip, inv_cfreq = self.get_clock_translation() + clock_to_print_time = self.mcu.clock_to_print_time + base_time = clock_to_print_time(base_mcu) + inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time + return base_time, base_chip, inv_freq + MIN_MSG_TIME = 0.100 # Printer class that controls ADXL345 chip @@ -191,7 +211,6 @@ class ADXL345: self.printer = config.get_printer() ADXLCommandHelper(config, self) self.query_rate = 0 - self.last_sequence = 0 am = {'x': (0, SCALE), 'y': (1, SCALE), 'z': (2, SCALE), '-x': (0, -SCALE), '-y': (1, -SCALE), '-z': (2, -SCALE)} axes_map = config.getlist('axes_map', ('x','y','z'), count=3) @@ -204,19 +223,21 @@ class ADXL345: # Measurement storage (accessed from background thread) self.lock = threading.Lock() self.raw_samples = [] - self.samples_start1 = self.samples_start2 = 0. # Setup mcu sensor_adxl345 bulk query code self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000) self.mcu = mcu = self.spi.get_mcu() self.oid = oid = mcu.create_oid() - self.query_adxl345_cmd = self.query_adxl345_end_cmd =None + self.query_adxl345_cmd = self.query_adxl345_end_cmd = None + self.query_adxl345_status_cmd = None mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d" % (oid, self.spi.get_oid())) mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0" % (oid,), on_restart=True) mcu.register_config_callback(self._build_config) - mcu.register_response(self._handle_adxl345_start, "adxl345_start", oid) mcu.register_response(self._handle_adxl345_data, "adxl345_data", oid) + # Clock tracking + self.last_sequence = self.last_limit_count = self.max_query_duration = 0 + self.clock_sync = ClockSyncRegression(self.mcu, 640) # API server endpoints self.api_dump = motion_report.APIDumpHelper( self.printer, self._api_update, self._api_startstop, 0.100) @@ -227,21 +248,17 @@ class ADXL345: wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name, self._handle_dump_adxl345) def _build_config(self): + cmdqueue = self.spi.get_command_queue() self.query_adxl345_cmd = self.mcu.lookup_command( - "query_adxl345 oid=%c clock=%u rest_ticks=%u", - cq=self.spi.get_command_queue()) + "query_adxl345 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue) self.query_adxl345_end_cmd = self.mcu.lookup_query_command( "query_adxl345 oid=%c clock=%u rest_ticks=%u", - "adxl345_end oid=%c end1_clock=%u end2_clock=%u" - " limit_count=%hu sequence=%hu", - oid=self.oid, cq=self.spi.get_command_queue()) - def _clock_to_print_time(self, clock): - return self.mcu.clock_to_print_time(self.mcu.clock32_to_clock64(clock)) - def _convert_sequence(self, sequence): - sequence = (self.last_sequence & ~0xffff) | sequence - if sequence < self.last_sequence: - sequence += 0x10000 - return sequence + "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" + " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue) + self.query_adxl345_status_cmd = self.mcu.lookup_query_command( + "query_adxl345_status oid=%c", + "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" + " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue) def read_reg(self, reg): params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00]) response = bytearray(params['response']) @@ -258,9 +275,6 @@ class ADXL345: # Measurement collection def is_measuring(self): return self.query_rate > 0 - def _handle_adxl345_start(self, params): - self.samples_start1 = self._clock_to_print_time(params['start1_clock']) - self.samples_start2 = self._clock_to_print_time(params['start2_clock']) def _handle_adxl345_data(self, params): with self.lock: self.raw_samples.append(params) @@ -268,27 +282,62 @@ class ADXL345: # Load variables to optimize inner loop below (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map last_sequence = self.last_sequence + time_base, chip_base, inv_freq = self.clock_sync.get_time_translation() # Process every message in raw_samples - count = 0 + count = seq = 0 samples = [None] * (len(raw_samples) * 8) for params in raw_samples: - seq = (last_sequence & ~0xffff) | params['sequence'] - if seq < last_sequence: - seq += 0x10000 - last_sequence = seq + seq_diff = (last_sequence - params['sequence']) & 0xffff + seq_diff -= (seq_diff & 0x8000) << 1 + seq = last_sequence - seq_diff d = bytearray(params['data']) len_d = len(d) - sdata = [(d[i] | (d[i+1] << 8)) - ((d[i+1] & 0x80) << 9) + sdata = [(d[i] | ((d[i+1] & 0x1f) << 8)) - ((d[i+1] & 0x10) << 9) for i in range(0, len_d-1, 2)] + msg_cdiff = seq * 8 - chip_base for i in range(len_d // 6): x = round(sdata[i*3 + x_pos] * x_scale, 6) y = round(sdata[i*3 + y_pos] * y_scale, 6) z = round(sdata[i*3 + z_pos] * z_scale, 6) - samples[count] = (seq * 8 + i, x, y, z) + ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6) + samples[count] = (ptime, x, y, z) count += 1 - self.last_sequence = last_sequence + self.clock_sync.set_last_chip_clock(seq * 8 + i) del samples[count:] return samples + def _update_clock(self, minclock=0): + # Query current state + for retry in range(5): + params = self.query_adxl345_status_cmd.send([self.oid], + minclock=minclock) + fifo = params['fifo'] & 0x7f + if fifo <= 32: + break + else: + raise self.printer.command_error("Unable to query adxl345 fifo") + mcu_clock = self.mcu.clock32_to_clock64(params['clock']) + sequence = (self.last_sequence & ~0xffff) | params['next_sequence'] + if sequence < self.last_sequence: + sequence += 0x10000 + self.last_sequence = sequence + buffered = params['buffered'] + limit_count = (self.last_limit_count & ~0xffff) | params['limit_count'] + if limit_count < self.last_limit_count: + limit_count += 0x10000 + self.last_limit_count = limit_count + duration = params['query_ticks'] + if duration > self.max_query_duration: + # Skip measurement as a high query time could skew clock tracking + self.max_query_duration = max(2 * self.max_query_duration, + self.mcu.seconds_to_clock(.000005)) + return + self.max_query_duration = 2 * duration + msg_count = sequence * 8 + buffered // 6 + fifo + # The "chip clock" is the message counter plus .5 for average + # inaccuracy of query responses and plus .5 for assumed offset + # of adxl345 hw processing time. + chip_clock = msg_count + 1 + self.clock_sync.update(mcu_clock + duration // 2, chip_clock) def _start_measurements(self): if self.is_measuring(): return @@ -305,19 +354,24 @@ class ADXL345: self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate]) self.set_reg(REG_FIFO_CTL, 0x80) # Setup samples - systime = self.printer.get_reactor().monotonic() - print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME - self.samples_start1 = self.samples_start2 = print_time - self.last_sequence = 0 with self.lock: self.raw_samples = [] # Start bulk reading + systime = self.printer.get_reactor().monotonic() + print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME reqclock = self.mcu.print_time_to_clock(print_time) rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate) self.query_rate = self.data_rate self.query_adxl345_cmd.send([self.oid, reqclock, rest_ticks], reqclock=reqclock) logging.info("ADXL345 starting '%s' measurements", self.name) + # Initialize clock tracking + self.last_sequence = 0 + self.last_limit_count = 0 + self.clock_sync.reset(reqclock, 0) + self.max_query_duration = 1 << 31 + self._update_clock(minclock=reqclock) + self.max_query_duration = 1 << 31 def _finish_measurements(self): if not self.is_measuring(): return @@ -326,17 +380,10 @@ class ADXL345: self.query_rate = 0 with self.lock: self.raw_samples = [] - # Generate results - end1_time = self._clock_to_print_time(params['end1_clock']) - end2_time = self._clock_to_print_time(params['end2_clock']) - end_sequence = self._convert_sequence(params['sequence']) - overflows = params['limit_count'] logging.info("ADXL345 finished '%s' measurements", self.name) - self.final_results = (end_sequence, overflows, - self.samples_start1, self.samples_start2, - end1_time, end2_time) # XXX # API interface def _api_update(self, eventtime): + self._update_clock() with self.lock: raw_samples = self.raw_samples self.raw_samples = [] @@ -345,7 +392,7 @@ class ADXL345: samples = self._extract_samples(raw_samples) if not samples: return {} - return {'data': samples} + return {'data': samples, 'overflows': self.last_limit_count} def _api_startstop(self, is_start): if is_start: self._start_measurements() @@ -353,14 +400,14 @@ class ADXL345: self._finish_measurements() def _handle_dump_adxl345(self, web_request): self.api_dump.add_client(web_request) - hdr = ('sequence', 'x_acceleration', 'y_acceleration', 'z_acceleration') + hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration') web_request.send({'header': hdr}) def start_internal_client(self): if self.is_measuring(): raise self.printer.command_error( "ADXL345 measurement already in progress") cconn = self.api_dump.add_internal_client() - return ADXL345QueryHelper(self.printer, self, cconn) + return ADXL345QueryHelper(self.printer, cconn) def load_config(config): return ADXL345(config) diff --git a/klippy/extras/resonance_tester.py b/klippy/extras/resonance_tester.py index eaa49db3..f66e229c 100644 --- a/klippy/extras/resonance_tester.py +++ b/klippy/extras/resonance_tester.py @@ -181,8 +181,6 @@ class ResonanceTester: gcmd.respond_info( "Writing raw accelerometer data to " "%s file" % (raw_name,)) - gcmd.respond_info("%s-axis accelerometer stats: %s" % ( - chip_axis, aclient.get_stats(),)) if helper is None: continue for chip_axis, chip_values in raw_values: |