forked from Klipper3d/klipper
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge remote-tracking branch 'upstream/master'
- Loading branch information
Showing
7 changed files
with
461 additions
and
530 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,10 +1,10 @@ | ||
| # Support for reading acceleration data from an adxl345 chip | ||
| # | ||
| # Copyright (C) 2020-2021 Kevin O'Connor <[email protected]> | ||
| # Copyright (C) 2020-2023 Kevin O'Connor <[email protected]> | ||
| # | ||
| # This file may be distributed under the terms of the GNU GPLv3 license. | ||
| import logging, time, collections, threading, multiprocessing, os | ||
| from . import bus, motion_report | ||
| import logging, time, collections, multiprocessing, os | ||
| from . import bus, bulk_sensor | ||
|
|
||
| # ADXL345 registers | ||
| REG_DEVID = 0x00 | ||
|
|
@@ -32,26 +32,29 @@ | |
|
|
||
| # Helper class to obtain measurements | ||
| class AccelQueryHelper: | ||
| def __init__(self, printer, cconn): | ||
| def __init__(self, printer): | ||
| self.printer = printer | ||
| self.cconn = cconn | ||
| self.is_finished = False | ||
| print_time = printer.lookup_object('toolhead').get_last_move_time() | ||
| self.request_start_time = self.request_end_time = print_time | ||
| self.samples = self.raw_samples = [] | ||
| self.msgs = [] | ||
| self.samples = [] | ||
| 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() | ||
| def _get_raw_samples(self): | ||
| raw_samples = self.cconn.get_messages() | ||
| if raw_samples: | ||
| self.raw_samples = raw_samples | ||
| return self.raw_samples | ||
| self.is_finished = True | ||
| def handle_batch(self, msg): | ||
| if self.is_finished: | ||
| return False | ||
| if len(self.msgs) >= 10000: | ||
| # Avoid filling up memory with too many samples | ||
| return False | ||
| self.msgs.append(msg) | ||
| return True | ||
| def has_valid_samples(self): | ||
| raw_samples = self._get_raw_samples() | ||
| for msg in raw_samples: | ||
| data = msg['params']['data'] | ||
| for msg in self.msgs: | ||
| data = msg['data'] | ||
| first_sample_time = data[0][0] | ||
| last_sample_time = data[-1][0] | ||
| if (first_sample_time > self.request_end_time | ||
|
|
@@ -60,21 +63,20 @@ def has_valid_samples(self): | |
| # The time intervals [first_sample_time, last_sample_time] | ||
| # and [request_start_time, request_end_time] have non-zero | ||
| # intersection. It is still theoretically possible that none | ||
| # of the samples from raw_samples fall into the time interval | ||
| # of the samples from msgs fall into the time interval | ||
| # [request_start_time, request_end_time] if it is too narrow | ||
| # or on very heavy data losses. In practice, that interval | ||
| # is at least 1 second, so this possibility is negligible. | ||
| return True | ||
| return False | ||
| def get_samples(self): | ||
| raw_samples = self._get_raw_samples() | ||
| if not raw_samples: | ||
| if not self.msgs: | ||
| return self.samples | ||
| total = sum([len(m['params']['data']) for m in raw_samples]) | ||
| total = sum([len(m['data']) for m in self.msgs]) | ||
| count = 0 | ||
| self.samples = samples = [None] * total | ||
| for msg in raw_samples: | ||
| for samp_time, x, y, z in msg['params']['data']: | ||
| for msg in self.msgs: | ||
| for samp_time, x, y, z in msg['data']: | ||
| if samp_time < self.request_start_time: | ||
| continue | ||
| if samp_time > self.request_end_time: | ||
|
|
@@ -173,77 +175,31 @@ def cmd_ACCELEROMETER_DEBUG_WRITE(self, gcmd): | |
| 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 | ||
| # Helper to read the axes_map parameter from the config | ||
| def read_axes_map(config): | ||
| am = {'x': (0, SCALE_XY), 'y': (1, SCALE_XY), 'z': (2, SCALE_Z), | ||
| '-x': (0, -SCALE_XY), '-y': (1, -SCALE_XY), '-z': (2, -SCALE_Z)} | ||
| axes_map = config.getlist('axes_map', ('x','y','z'), count=3) | ||
| if any([a not in am for a in axes_map]): | ||
| raise config.error("Invalid axes_map parameter") | ||
| return [am[a.strip()] for a in axes_map] | ||
|
|
||
| MIN_MSG_TIME = 0.100 | ||
|
|
||
| BYTES_PER_SAMPLE = 5 | ||
| SAMPLES_PER_BLOCK = 10 | ||
|
|
||
| BATCH_UPDATES = 0.100 | ||
|
|
||
| # Printer class that controls ADXL345 chip | ||
| class ADXL345: | ||
| def __init__(self, config): | ||
| self.printer = config.get_printer() | ||
| AccelCommandHelper(config, self) | ||
| self.query_rate = 0 | ||
| am = {'x': (0, SCALE_XY), 'y': (1, SCALE_XY), 'z': (2, SCALE_Z), | ||
| '-x': (0, -SCALE_XY), '-y': (1, -SCALE_XY), '-z': (2, -SCALE_Z)} | ||
| axes_map = config.getlist('axes_map', ('x','y','z'), count=3) | ||
| if any([a not in am for a in axes_map]): | ||
| raise config.error("Invalid adxl345 axes_map parameter") | ||
| self.axes_map = [am[a.strip()] for a in axes_map] | ||
| self.axes_map = read_axes_map(config) | ||
| self.data_rate = config.getint('rate', 3200) | ||
| if self.data_rate not in QUERY_RATES: | ||
| raise config.error("Invalid rate parameter: %d" % (self.data_rate,)) | ||
| # Measurement storage (accessed from background thread) | ||
| self.lock = threading.Lock() | ||
| self.raw_samples = [] | ||
| # 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() | ||
|
|
@@ -255,18 +211,21 @@ def __init__(self, config): | |
| 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_data, "adxl345_data", oid) | ||
| self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid) | ||
| # Clock tracking | ||
| self.last_sequence = self.max_query_duration = 0 | ||
| self.last_limit_count = self.last_error_count = 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) | ||
| chip_smooth = self.data_rate * BATCH_UPDATES * 2 | ||
| self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth) | ||
| self.clock_updater = bulk_sensor.ChipClockUpdater(self.clock_sync, | ||
| BYTES_PER_SAMPLE) | ||
| self.last_error_count = 0 | ||
| # Process messages in batches | ||
| self.batch_bulk = bulk_sensor.BatchBulkHelper( | ||
| self.printer, self._process_batch, | ||
| self._start_measurements, self._finish_measurements, BATCH_UPDATES) | ||
| self.name = config.get_name().split()[-1] | ||
| wh = self.printer.lookup_object('webhooks') | ||
| wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name, | ||
| self._handle_dump_adxl345) | ||
| hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration') | ||
| self.batch_bulk.add_mux_endpoint("adxl345/dump_adxl345", "sensor", | ||
| self.name, {'header': hdr}) | ||
| def _build_config(self): | ||
| cmdqueue = self.spi.get_command_queue() | ||
| self.query_adxl345_cmd = self.mcu.lookup_command( | ||
|
|
@@ -292,16 +251,15 @@ def set_reg(self, reg, val, minclock=0): | |
| "This is generally indicative of connection problems " | ||
| "(e.g. faulty wiring) or a faulty adxl345 chip." % ( | ||
| reg, val, stored_val)) | ||
| # Measurement collection | ||
| def is_measuring(self): | ||
| return self.query_rate > 0 | ||
| def _handle_adxl345_data(self, params): | ||
| with self.lock: | ||
| self.raw_samples.append(params) | ||
| def start_internal_client(self): | ||
| aqh = AccelQueryHelper(self.printer) | ||
| self.batch_bulk.add_client(aqh.handle_batch) | ||
| return aqh | ||
| # Measurement decoding | ||
| def _extract_samples(self, raw_samples): | ||
| # 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 | ||
| last_sequence = self.clock_updater.get_last_sequence() | ||
| time_base, chip_base, inv_freq = self.clock_sync.get_time_translation() | ||
| # Process every message in raw_samples | ||
| count = seq = 0 | ||
|
|
@@ -342,29 +300,9 @@ def _update_clock(self, minclock=0): | |
| break | ||
| else: | ||
| raise self.printer.command_error("Unable to query adxl345 fifo") | ||
| mcu_clock = self.mcu.clock32_to_clock64(params['clock']) | ||
| seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff | ||
| self.last_sequence += seq_diff | ||
| buffered = params['buffered'] | ||
| lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff | ||
| self.last_limit_count += lc_diff | ||
| 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 = (self.last_sequence * SAMPLES_PER_BLOCK | ||
| + buffered // BYTES_PER_SAMPLE + 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) | ||
| self.clock_updater.update_clock(params) | ||
| # Start, stop, and process message batches | ||
| def _start_measurements(self): | ||
| if self.is_measuring(): | ||
| return | ||
| # In case of miswiring, testing ADXL345 device ID prevents treating | ||
| # noise or wrong signal as a correctly initialized device | ||
| dev_id = self.read_reg(REG_DEVID) | ||
|
|
@@ -380,59 +318,35 @@ def _start_measurements(self): | |
| self.set_reg(REG_FIFO_CTL, 0x00) | ||
| self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate]) | ||
| self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL) | ||
| # Setup samples | ||
| with self.lock: | ||
| self.raw_samples = [] | ||
| # Start bulk reading | ||
| self.bulk_queue.clear_samples() | ||
| 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 = self.last_error_count = 0 | ||
| self.clock_sync.reset(reqclock, 0) | ||
| self.max_query_duration = 1 << 31 | ||
| self.clock_updater.note_start(reqclock) | ||
| self._update_clock(minclock=reqclock) | ||
| self.max_query_duration = 1 << 31 | ||
| self.clock_updater.clear_duration_filter() | ||
| self.last_error_count = 0 | ||
| def _finish_measurements(self): | ||
| if not self.is_measuring(): | ||
| return | ||
| # Halt bulk reading | ||
| params = self.query_adxl345_end_cmd.send([self.oid, 0, 0]) | ||
| self.query_rate = 0 | ||
| with self.lock: | ||
| self.raw_samples = [] | ||
| self.bulk_queue.clear_samples() | ||
| logging.info("ADXL345 finished '%s' measurements", self.name) | ||
| # API interface | ||
| def _api_update(self, eventtime): | ||
| def _process_batch(self, eventtime): | ||
| self._update_clock() | ||
| with self.lock: | ||
| raw_samples = self.raw_samples | ||
| self.raw_samples = [] | ||
| raw_samples = self.bulk_queue.pull_samples() | ||
| if not raw_samples: | ||
| return {} | ||
| samples = self._extract_samples(raw_samples) | ||
| if not samples: | ||
| return {} | ||
| return {'data': samples, 'errors': self.last_error_count, | ||
| 'overflows': self.last_limit_count} | ||
| def _api_startstop(self, is_start): | ||
| if is_start: | ||
| self._start_measurements() | ||
| else: | ||
| self._finish_measurements() | ||
| def _handle_dump_adxl345(self, web_request): | ||
| self.api_dump.add_client(web_request) | ||
| hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration') | ||
| web_request.send({'header': hdr}) | ||
| def start_internal_client(self): | ||
| cconn = self.api_dump.add_internal_client() | ||
| return AccelQueryHelper(self.printer, cconn) | ||
| 'overflows': self.clock_updater.get_last_limit_count()} | ||
|
|
||
| def load_config(config): | ||
| return ADXL345(config) | ||
|
|
||
Oops, something went wrong.