3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

bulk_sensor: Add new ChipClockUpdater helper class 

All the accelerometers use a standard response for their query_status
messages.  Create a common helper class to process those responses.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2023-12-16 13:01:34 -05:00
parent e67cbbe5c1
commit d6a4669ce0
4 changed files with 80 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
klippy/extras/adxl345.py
View File

@ -1,6 +1,6 @@
# Support for reading acceleration data from an adxl345 chip # Support for reading acceleration data from an adxl345 chip
# #
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2020-2023 Kevin O'Connor <kevin@koconnor.net>
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # This file may be distributed under the terms of the GNU GPLv3 license.
import logging, time, collections, multiprocessing, os import logging, time, collections, multiprocessing, os
@ -208,10 +208,11 @@ class ADXL345:
mcu.register_config_callback(self._build_config) mcu.register_config_callback(self._build_config)
self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid) self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid)
# Clock tracking # Clock tracking
self.last_sequence = self.max_query_duration = 0
self.last_limit_count = self.last_error_count = 0
chip_smooth = self.data_rate * API_UPDATES * 2 chip_smooth = self.data_rate * API_UPDATES * 2
self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth) 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
# API server endpoints # API server endpoints
self.api_dump = motion_report.APIDumpHelper( self.api_dump = motion_report.APIDumpHelper(
self.printer, self._api_update, self._api_startstop, API_UPDATES) self.printer, self._api_update, self._api_startstop, API_UPDATES)
@ -250,7 +251,7 @@ class ADXL345:
def _extract_samples(self, raw_samples): def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below # Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map (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() time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
# Process every message in raw_samples # Process every message in raw_samples
count = seq = 0 count = seq = 0
@ -291,26 +292,7 @@ class ADXL345:
break break
else: else:
raise self.printer.command_error("Unable to query adxl345 fifo") raise self.printer.command_error("Unable to query adxl345 fifo")
mcu_clock = self.mcu.clock32_to_clock64(params['clock']) self.clock_updater.update_clock(params)
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)
def _start_measurements(self): def _start_measurements(self):
if self.is_measuring(): if self.is_measuring():
return return
@ -340,12 +322,10 @@ class ADXL345:
reqclock=reqclock) reqclock=reqclock)
logging.info("ADXL345 starting '%s' measurements", self.name) logging.info("ADXL345 starting '%s' measurements", self.name)
# Initialize clock tracking # Initialize clock tracking
self.last_sequence = 0 self.clock_updater.note_start(reqclock)
self.last_limit_count = self.last_error_count = 0
self.clock_sync.reset(reqclock, 0)
self.max_query_duration = 1 << 31
self._update_clock(minclock=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): def _finish_measurements(self):
if not self.is_measuring(): if not self.is_measuring():
return return
@ -364,7 +344,7 @@ class ADXL345:
if not samples: if not samples:
return {} return {}
return {'data': samples, 'errors': self.last_error_count, return {'data': samples, 'errors': self.last_error_count,
'overflows': self.last_limit_count} 'overflows': self.clock_updater.get_last_limit_count()}
def _api_startstop(self, is_start): def _api_startstop(self, is_start):
if is_start: if is_start:
self._start_measurements() self._start_measurements()

48
klippy/extras/bulk_sensor.py
View File

@ -71,3 +71,51 @@ class ClockSyncRegression:
base_time = clock_to_print_time(base_mcu) base_time = clock_to_print_time(base_mcu)
inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time
return base_time, base_chip, inv_freq return base_time, base_chip, inv_freq
MAX_BULK_MSG_SIZE = 52
# Handle common periodic chip status query responses
class ChipClockUpdater:
def __init__(self, clock_sync, bytes_per_sample):
self.clock_sync = clock_sync
self.bytes_per_sample = bytes_per_sample
self.samples_per_block = MAX_BULK_MSG_SIZE // bytes_per_sample
self.mcu = clock_sync.mcu
self.last_sequence = self.max_query_duration = 0
self.last_limit_count = 0
def get_last_sequence(self):
return self.last_sequence
def get_last_limit_count(self):
return self.last_limit_count
def clear_duration_filter(self):
self.max_query_duration = 1 << 31
def note_start(self, reqclock):
self.last_sequence = 0
self.last_limit_count = 0
self.clock_sync.reset(reqclock, 0)
self.clear_duration_filter()
def update_clock(self, params):
# Handle a status response message of the form:
# adxl345_status oid=x clock=x query_ticks=x next_sequence=x
# buffered=x fifo=x limit_count=x
fifo = params['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 * self.samples_per_block
+ buffered // self.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 hardware processing time.
chip_clock = msg_count + 1
self.clock_sync.update(mcu_clock + duration // 2, chip_clock)

45
klippy/extras/lis2dw.py
View File

@ -63,10 +63,11 @@ class LIS2DW:
mcu.register_config_callback(self._build_config) mcu.register_config_callback(self._build_config)
self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "lis2dw_data", oid) self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "lis2dw_data", oid)
# Clock tracking # Clock tracking
self.last_sequence = self.max_query_duration = 0
self.last_limit_count = self.last_error_count = 0
chip_smooth = self.data_rate * API_UPDATES * 2 chip_smooth = self.data_rate * API_UPDATES * 2
self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth) 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
# API server endpoints # API server endpoints
self.api_dump = motion_report.APIDumpHelper( self.api_dump = motion_report.APIDumpHelper(
self.printer, self._api_update, self._api_startstop, API_UPDATES) self.printer, self._api_update, self._api_startstop, API_UPDATES)
@ -106,7 +107,7 @@ class LIS2DW:
def _extract_samples(self, raw_samples): def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below # Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map (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() time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
# Process every message in raw_samples # Process every message in raw_samples
count = seq = 0 count = seq = 0
@ -140,35 +141,9 @@ class LIS2DW:
del samples[count:] del samples[count:]
return samples return samples
def _update_clock(self, minclock=0): def _update_clock(self, minclock=0):
# Query current state
for retry in range(5):
params = self.query_lis2dw_status_cmd.send([self.oid], params = self.query_lis2dw_status_cmd.send([self.oid],
minclock=minclock) minclock=minclock)
fifo = params['fifo'] & 0x1f self.clock_updater.update_clock(params)
if fifo <= 32:
break
else:
raise self.printer.command_error("Unable to query lis2dw 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 lis2dw hw processing time.
chip_clock = msg_count + 1
self.clock_sync.update(mcu_clock + duration // 2, chip_clock)
def _start_measurements(self): def _start_measurements(self):
if self.is_measuring(): if self.is_measuring():
return return
@ -203,12 +178,10 @@ class LIS2DW:
reqclock=reqclock) reqclock=reqclock)
logging.info("LIS2DW starting '%s' measurements", self.name) logging.info("LIS2DW starting '%s' measurements", self.name)
# Initialize clock tracking # Initialize clock tracking
self.last_sequence = 0 self.clock_updater.note_start(reqclock)
self.last_limit_count = self.last_error_count = 0
self.clock_sync.reset(reqclock, 0)
self.max_query_duration = 1 << 31
self._update_clock(minclock=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): def _finish_measurements(self):
if not self.is_measuring(): if not self.is_measuring():
return return
@ -228,7 +201,7 @@ class LIS2DW:
if not samples: if not samples:
return {} return {}
return {'data': samples, 'errors': self.last_error_count, return {'data': samples, 'errors': self.last_error_count,
'overflows': self.last_limit_count} 'overflows': self.clock_updater.get_last_limit_count()}
def _api_startstop(self, is_start): def _api_startstop(self, is_start):
if is_start: if is_start:
self._start_measurements() self._start_measurements()

45
klippy/extras/mpu9250.py
View File

@ -80,10 +80,11 @@ class MPU9250:
mcu.register_config_callback(self._build_config) mcu.register_config_callback(self._build_config)
self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "mpu9250_data", oid) self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "mpu9250_data", oid)
# Clock tracking # Clock tracking
self.last_sequence = self.max_query_duration = 0
self.last_limit_count = self.last_error_count = 0
chip_smooth = self.data_rate * API_UPDATES * 2 chip_smooth = self.data_rate * API_UPDATES * 2
self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth) 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
# API server endpoints # API server endpoints
self.api_dump = motion_report.APIDumpHelper( self.api_dump = motion_report.APIDumpHelper(
self.printer, self._api_update, self._api_startstop, API_UPDATES) self.printer, self._api_update, self._api_startstop, API_UPDATES)
@ -120,7 +121,7 @@ class MPU9250:
def _extract_samples(self, raw_samples): def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below # Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map (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() time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
# Process every message in raw_samples # Process every message in raw_samples
count = seq = 0 count = seq = 0
@ -152,35 +153,9 @@ class MPU9250:
return samples return samples
def _update_clock(self, minclock=0): def _update_clock(self, minclock=0):
# Query current state
for retry in range(5):
params = self.query_mpu9250_status_cmd.send([self.oid], params = self.query_mpu9250_status_cmd.send([self.oid],
minclock=minclock) minclock=minclock)
fifo = params['fifo'] & 0x1fff self.clock_updater.update_clock(params)
if fifo <= FIFO_SIZE:
break
else:
raise self.printer.command_error("Unable to query mpu9250 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 mpu9250 hw processing time.
chip_clock = msg_count + 1
self.clock_sync.update(mcu_clock + duration // 2, chip_clock)
def _start_measurements(self): def _start_measurements(self):
if self.is_measuring(): if self.is_measuring():
return return
@ -215,12 +190,10 @@ class MPU9250:
reqclock=reqclock) reqclock=reqclock)
logging.info("MPU9250 starting '%s' measurements", self.name) logging.info("MPU9250 starting '%s' measurements", self.name)
# Initialize clock tracking # Initialize clock tracking
self.last_sequence = 0 self.clock_updater.note_start(reqclock)
self.last_limit_count = self.last_error_count = 0
self.clock_sync.reset(reqclock, 0)
self.max_query_duration = 1 << 31
self._update_clock(minclock=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): def _finish_measurements(self):
if not self.is_measuring(): if not self.is_measuring():
return return
@ -242,7 +215,7 @@ class MPU9250:
if not samples: if not samples:
return {} return {}
return {'data': samples, 'errors': self.last_error_count, return {'data': samples, 'errors': self.last_error_count,
'overflows': self.last_limit_count} 'overflows': self.clock_updater.get_last_limit_count()}
def _api_startstop(self, is_start): def _api_startstop(self, is_start):
if is_start: if is_start:
self._start_measurements() self._start_measurements()