diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py index 76fd4ca4..b91224d5 100644 --- a/klippy/extras/adxl345.py +++ b/klippy/extras/adxl345.py @@ -187,7 +187,7 @@ def read_axes_map(config): MIN_MSG_TIME = 0.100 BYTES_PER_SAMPLE = 5 -SAMPLES_PER_BLOCK = 10 +SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE BATCH_UPDATES = 0.100 @@ -205,13 +205,12 @@ class ADXL345: self.mcu = mcu = self.spi.get_mcu() self.oid = oid = mcu.create_oid() self.query_adxl345_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) - self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid) # Clock tracking chip_smooth = self.data_rate * BATCH_UPDATES * 2 self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth) @@ -230,10 +229,8 @@ class ADXL345: cmdqueue = self.spi.get_command_queue() self.query_adxl345_cmd = self.mcu.lookup_command( "query_adxl345 oid=%c clock=%u rest_ticks=%u", 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) + self.clock_updater.setup_query_command( + self.mcu, "query_adxl345_status oid=%c", oid=self.oid, cq=cmdqueue) def read_reg(self, reg): params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00]) response = bytearray(params['response']) @@ -286,17 +283,6 @@ class ADXL345: self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + 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") - self.clock_updater.update_clock(params) # Start, stop, and process message batches def _start_measurements(self): # In case of miswiring, testing ADXL345 device ID prevents treating @@ -325,8 +311,6 @@ class ADXL345: logging.info("ADXL345 starting '%s' measurements", self.name) # Initialize clock tracking self.clock_updater.note_start(reqclock) - self._update_clock(minclock=reqclock) - self.clock_updater.clear_duration_filter() self.last_error_count = 0 def _finish_measurements(self): # Halt bulk reading @@ -334,7 +318,7 @@ class ADXL345: self.bulk_queue.clear_samples() logging.info("ADXL345 finished '%s' measurements", self.name) def _process_batch(self, eventtime): - self._update_clock() + self.clock_updater.update_clock() raw_samples = self.bulk_queue.pull_samples() if not raw_samples: return {} @@ -342,7 +326,7 @@ class ADXL345: if not samples: return {} return {'data': samples, 'errors': self.last_error_count, - 'overflows': self.clock_updater.get_last_limit_count()} + 'overflows': self.clock_updater.get_last_overflows()} def load_config(config): return ADXL345(config) diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py index 163168d0..23f402a7 100644 --- a/klippy/extras/angle.py +++ b/klippy/extras/angle.py @@ -412,7 +412,7 @@ class HelperTLE5012B: self._write_reg(reg, val) BYTES_PER_SAMPLE = 3 -SAMPLES_PER_BLOCK = 16 +SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE SAMPLE_PERIOD = 0.000400 BATCH_UPDATES = 0.100 @@ -445,7 +445,7 @@ class Angle: "query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0" % (oid,), on_restart=True) mcu.register_config_callback(self._build_config) - self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "spi_angle_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid) # Process messages in batches self.batch_bulk = bulk_sensor.BatchBulkHelper( self.printer, self._process_batch, diff --git a/klippy/extras/bulk_sensor.py b/klippy/extras/bulk_sensor.py index 8d0c0541..df5a5da2 100644 --- a/klippy/extras/bulk_sensor.py +++ b/klippy/extras/bulk_sensor.py @@ -114,7 +114,7 @@ class BatchWebhooksClient: # Helper class to store incoming messages in a queue class BulkDataQueue: - def __init__(self, mcu, msg_name, oid): + def __init__(self, mcu, msg_name="sensor_bulk_data", oid=None): # Measurement storage (accessed from background thread) self.lock = threading.Lock() self.raw_samples = [] @@ -206,31 +206,37 @@ class ChipClockUpdater: 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 + self.last_overflows = 0 + self.mcu = self.oid = self.query_status_cmd = None + def setup_query_command(self, mcu, msgformat, oid, cq): + self.mcu = mcu + self.oid = oid + self.query_status_cmd = self.mcu.lookup_query_command( + msgformat, "sensor_bulk_status oid=%c clock=%u query_ticks=%u" + " next_sequence=%hu buffered=%u possible_overflows=%hu", + oid=oid, cq=cq) def get_last_sequence(self): return self.last_sequence - def get_last_limit_count(self): - return self.last_limit_count + def get_last_overflows(self): + return self.last_overflows 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.last_overflows = 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'] + self.update_clock(minclock=reqclock) + self.clear_duration_filter() + def update_clock(self, minclock=0): + params = self.query_status_cmd.send([self.oid], minclock=minclock) 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 + po_diff = (params['possible_overflows'] - self.last_overflows) & 0xffff + self.last_overflows += po_diff duration = params['query_ticks'] if duration > self.max_query_duration: # Skip measurement as a high query time could skew clock tracking @@ -239,7 +245,7 @@ class ChipClockUpdater: return self.max_query_duration = 2 * duration msg_count = (self.last_sequence * self.samples_per_block - + buffered // self.bytes_per_sample + fifo) + + buffered // self.bytes_per_sample) # 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. diff --git a/klippy/extras/lis2dw.py b/klippy/extras/lis2dw.py index a7fe54d7..74911e6f 100644 --- a/klippy/extras/lis2dw.py +++ b/klippy/extras/lis2dw.py @@ -33,7 +33,7 @@ SCALE = FREEFALL_ACCEL * 1.952 / 4 MIN_MSG_TIME = 0.100 BYTES_PER_SAMPLE = 6 -SAMPLES_PER_BLOCK = 8 +SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE BATCH_UPDATES = 0.100 @@ -49,13 +49,12 @@ class LIS2DW: self.mcu = mcu = self.spi.get_mcu() self.oid = oid = mcu.create_oid() self.query_lis2dw_cmd = None - self.query_lis2dw_status_cmd = None mcu.add_config_cmd("config_lis2dw oid=%d spi_oid=%d" % (oid, self.spi.get_oid())) mcu.add_config_cmd("query_lis2dw oid=%d clock=0 rest_ticks=0" % (oid,), on_restart=True) mcu.register_config_callback(self._build_config) - self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "lis2dw_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid) # Clock tracking chip_smooth = self.data_rate * BATCH_UPDATES * 2 self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth) @@ -75,10 +74,8 @@ class LIS2DW: cmdqueue = self.spi.get_command_queue() self.query_lis2dw_cmd = self.mcu.lookup_command( "query_lis2dw oid=%c clock=%u rest_ticks=%u", cq=cmdqueue) - self.query_lis2dw_status_cmd = self.mcu.lookup_query_command( - "query_lis2dw_status oid=%c", - "lis2dw_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" - " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue) + self.clock_updater.setup_query_command( + self.mcu, "query_lis2dw_status oid=%c", oid=self.oid, cq=cmdqueue) def read_reg(self, reg): params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00]) response = bytearray(params['response']) @@ -133,10 +130,6 @@ class LIS2DW: self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i) del samples[count:] return samples - def _update_clock(self, minclock=0): - params = self.query_lis2dw_status_cmd.send([self.oid], - minclock=minclock) - self.clock_updater.update_clock(params) # Start, stop, and process message batches def _start_measurements(self): # In case of miswiring, testing LIS2DW device ID prevents treating @@ -170,8 +163,6 @@ class LIS2DW: logging.info("LIS2DW starting '%s' measurements", self.name) # Initialize clock tracking self.clock_updater.note_start(reqclock) - self._update_clock(minclock=reqclock) - self.clock_updater.clear_duration_filter() self.last_error_count = 0 def _finish_measurements(self): # Halt bulk reading @@ -180,7 +171,7 @@ class LIS2DW: logging.info("LIS2DW finished '%s' measurements", self.name) self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00) def _process_batch(self, eventtime): - self._update_clock() + self.clock_updater.update_clock() raw_samples = self.bulk_queue.pull_samples() if not raw_samples: return {} @@ -188,7 +179,7 @@ class LIS2DW: if not samples: return {} return {'data': samples, 'errors': self.last_error_count, - 'overflows': self.clock_updater.get_last_limit_count()} + 'overflows': self.clock_updater.get_last_overflows()} def load_config(config): return LIS2DW(config) diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py index 41376dc3..4626e1c0 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -50,7 +50,7 @@ FIFO_SIZE = 512 MIN_MSG_TIME = 0.100 BYTES_PER_SAMPLE = 6 -SAMPLES_PER_BLOCK = 8 +SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE BATCH_UPDATES = 0.100 @@ -70,9 +70,8 @@ class MPU9250: self.mcu = mcu = self.i2c.get_mcu() self.oid = oid = mcu.create_oid() self.query_mpu9250_cmd = None - self.query_mpu9250_status_cmd = None mcu.register_config_callback(self._build_config) - self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "mpu9250_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid) # Clock tracking chip_smooth = self.data_rate * BATCH_UPDATES * 2 self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth) @@ -95,10 +94,8 @@ class MPU9250: % (self.oid,), on_restart=True) self.query_mpu9250_cmd = self.mcu.lookup_command( "query_mpu9250 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue) - self.query_mpu9250_status_cmd = self.mcu.lookup_query_command( - "query_mpu9250_status oid=%c", - "mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" - " buffered=%c fifo=%u limit_count=%hu", oid=self.oid, cq=cmdqueue) + self.clock_updater.setup_query_command( + self.mcu, "query_mpu9250_status oid=%c", oid=self.oid, cq=cmdqueue) def read_reg(self, reg): params = self.i2c.i2c_read([reg], 1) return bytearray(params['response'])[0] @@ -142,11 +139,6 @@ class MPU9250: self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i) del samples[count:] return samples - - def _update_clock(self, minclock=0): - params = self.query_mpu9250_status_cmd.send([self.oid], - minclock=minclock) - self.clock_updater.update_clock(params) # Start, stop, and process message batches def _start_measurements(self): # In case of miswiring, testing MPU9250 device ID prevents treating @@ -184,8 +176,6 @@ class MPU9250: logging.info("MPU9250 starting '%s' measurements", self.name) # Initialize clock tracking self.clock_updater.note_start(reqclock) - self._update_clock(minclock=reqclock) - self.clock_updater.clear_duration_filter() self.last_error_count = 0 def _finish_measurements(self): # Halt bulk reading @@ -195,7 +185,7 @@ class MPU9250: self.set_reg(REG_PWR_MGMT_1, SET_PWR_MGMT_1_SLEEP) self.set_reg(REG_PWR_MGMT_2, SET_PWR_MGMT_2_OFF) def _process_batch(self, eventtime): - self._update_clock() + self.clock_updater.update_clock() raw_samples = self.bulk_queue.pull_samples() if not raw_samples: return {} @@ -203,7 +193,7 @@ class MPU9250: if not samples: return {} return {'data': samples, 'errors': self.last_error_count, - 'overflows': self.clock_updater.get_last_limit_count()} + 'overflows': self.clock_updater.get_last_overflows()} def load_config(config): return MPU9250(config) diff --git a/src/Kconfig b/src/Kconfig index aaf50653..91376910 100644 --- a/src/Kconfig +++ b/src/Kconfig @@ -112,6 +112,10 @@ config WANT_SOFTWARE_SPI bool depends on HAVE_GPIO && HAVE_GPIO_SPI default y +config NEED_SENSOR_BULK + bool + depends on WANT_SENSORS || WANT_LIS2DW + default y menu "Optional features (to reduce code size)" depends on HAVE_LIMITED_CODE_SIZE config WANT_GPIO_BITBANGING diff --git a/src/Makefile b/src/Makefile index 8d771f9e..eddad978 100644 --- a/src/Makefile +++ b/src/Makefile @@ -16,6 +16,7 @@ src-$(CONFIG_WANT_SOFTWARE_SPI) += spi_software.c src-$(CONFIG_WANT_SOFTWARE_I2C) += i2c_software.c sensors-src-$(CONFIG_HAVE_GPIO_SPI) := thermocouple.c sensor_adxl345.c \ sensor_angle.c -src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c sensors-src-$(CONFIG_HAVE_GPIO_I2C) += sensor_mpu9250.c src-$(CONFIG_WANT_SENSORS) += $(sensors-src-y) +src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c +src-$(CONFIG_NEED_SENSOR_BULK) += sensor_bulk.c diff --git a/src/sensor_adxl345.c b/src/sensor_adxl345.c index 5ec3945e..8cd471ef 100644 --- a/src/sensor_adxl345.c +++ b/src/sensor_adxl345.c @@ -10,15 +10,15 @@ #include "basecmd.h" // oid_alloc #include "command.h" // DECL_COMMAND #include "sched.h" // DECL_TASK +#include "sensor_bulk.h" // sensor_bulk_report #include "spicmds.h" // spidev_transfer struct adxl345 { struct timer timer; uint32_t rest_ticks; struct spidev_s *spi; - uint16_t sequence, limit_count; - uint8_t flags, data_count; - uint8_t data[50]; + uint8_t flags; + struct sensor_bulk sb; }; enum { @@ -47,27 +47,6 @@ command_config_adxl345(uint32_t *args) } DECL_COMMAND(command_config_adxl345, "config_adxl345 oid=%c spi_oid=%c"); -// Report local measurement buffer -static void -adxl_report(struct adxl345 *ax, uint8_t oid) -{ - sendf("adxl345_data oid=%c sequence=%hu data=%*s" - , oid, ax->sequence, ax->data_count, ax->data); - ax->data_count = 0; - ax->sequence++; -} - -// Report buffer and fifo status -static void -adxl_status(struct adxl345 *ax, uint_fast8_t oid - , uint32_t time1, uint32_t time2, uint_fast8_t fifo) -{ - sendf("adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" - " buffered=%c fifo=%c limit_count=%hu" - , oid, time1, time2-time1, ax->sequence - , ax->data_count, fifo, ax->limit_count); -} - // Helper code to reschedule the adxl345_event() timer static void adxl_reschedule_timer(struct adxl345 *ax) @@ -87,6 +66,8 @@ adxl_reschedule_timer(struct adxl345 *ax) #define SET_FIFO_CTL 0x90 +#define BYTES_PER_SAMPLE 5 + // Query accelerometer data static void adxl_query(struct adxl345 *ax, uint8_t oid) @@ -96,7 +77,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid) spidev_transfer(ax->spi, 1, sizeof(msg), msg); // Extract x, y, z measurements uint_fast8_t fifo_status = msg[8] & ~0x80; // Ignore trigger bit - uint8_t *d = &ax->data[ax->data_count]; + uint8_t *d = &ax->sb.data[ax->sb.data_count]; if (((msg[2] & 0xf0) && (msg[2] & 0xf0) != 0xf0) || ((msg[4] & 0xf0) && (msg[4] & 0xf0) != 0xf0) || ((msg[6] & 0xf0) && (msg[6] & 0xf0) != 0xf0) @@ -112,12 +93,12 @@ adxl_query(struct adxl345 *ax, uint8_t oid) d[3] = (msg[2] & 0x1f) | (msg[6] << 5); // x high bits and z high bits d[4] = (msg[4] & 0x1f) | ((msg[6] << 2) & 0x60); // y high and z high } - ax->data_count += 5; - if (ax->data_count + 5 > ARRAY_SIZE(ax->data)) - adxl_report(ax, oid); + ax->sb.data_count += BYTES_PER_SAMPLE; + if (ax->sb.data_count + BYTES_PER_SAMPLE > ARRAY_SIZE(ax->sb.data)) + sensor_bulk_report(&ax->sb, oid); // Check fifo status if (fifo_status >= 31) - ax->limit_count++; + ax->sb.possible_overflows++; if (fifo_status > 1 && fifo_status <= 32) { // More data in fifo - wake this task again sched_wake_task(&adxl345_wake); @@ -166,8 +147,7 @@ command_query_adxl345(uint32_t *args) ax->timer.waketime = args[1]; ax->rest_ticks = args[2]; ax->flags = AX_HAVE_START; - ax->sequence = ax->limit_count = 0; - ax->data_count = 0; + sensor_bulk_reset(&ax->sb); sched_add_timer(&ax->timer); } DECL_COMMAND(command_query_adxl345, @@ -178,10 +158,17 @@ command_query_adxl345_status(uint32_t *args) { struct adxl345 *ax = oid_lookup(args[0], command_config_adxl345); uint8_t msg[2] = { AR_FIFO_STATUS | AM_READ, 0x00 }; + uint32_t time1 = timer_read_time(); spidev_transfer(ax->spi, 1, sizeof(msg), msg); uint32_t time2 = timer_read_time(); - adxl_status(ax, args[0], time1, time2, msg[1]); + + uint_fast8_t fifo_status = msg[1] & ~0x80; // Ignore trigger bit + if (fifo_status > 32) + // Query error - don't send response - host will retry + return; + sensor_bulk_status(&ax->sb, args[0], time1, time2-time1 + , fifo_status * BYTES_PER_SAMPLE); } DECL_COMMAND(command_query_adxl345_status, "query_adxl345_status oid=%c"); diff --git a/src/sensor_angle.c b/src/sensor_angle.c index 865670b8..54caecc2 100644 --- a/src/sensor_angle.c +++ b/src/sensor_angle.c @@ -10,6 +10,7 @@ #include "board/irq.h" // irq_disable #include "command.h" // DECL_COMMAND #include "sched.h" // DECL_TASK +#include "sensor_bulk.h" // sensor_bulk_report #include "spicmds.h" // spidev_transfer enum { SA_CHIP_A1333, SA_CHIP_AS5047D, SA_CHIP_TLE5012B, SA_CHIP_MAX }; @@ -29,15 +30,16 @@ struct spi_angle { struct timer timer; uint32_t rest_ticks; struct spidev_s *spi; - uint16_t sequence; - uint8_t flags, chip_type, data_count, time_shift, overflow; - uint8_t data[48]; + uint8_t flags, chip_type, time_shift, overflow; + struct sensor_bulk sb; }; enum { SA_PENDING = 1<<2, }; +#define BYTES_PER_SAMPLE 3 + static struct task_wake angle_wake; // Event handler that wakes spi_angle_task() periodically @@ -72,32 +74,22 @@ command_config_spi_angle(uint32_t *args) DECL_COMMAND(command_config_spi_angle, "config_spi_angle oid=%c spi_oid=%c spi_angle_type=%c"); -// Report local measurement buffer -static void -angle_report(struct spi_angle *sa, uint8_t oid) -{ - sendf("spi_angle_data oid=%c sequence=%hu data=%*s" - , oid, sa->sequence, sa->data_count, sa->data); - sa->data_count = 0; - sa->sequence++; -} - // Send spi_angle_data message if buffer is full static void angle_check_report(struct spi_angle *sa, uint8_t oid) { - if (sa->data_count + 3 > ARRAY_SIZE(sa->data)) - angle_report(sa, oid); + if (sa->sb.data_count + BYTES_PER_SAMPLE > ARRAY_SIZE(sa->sb.data)) + sensor_bulk_report(&sa->sb, oid); } // Add an entry to the measurement buffer static void angle_add(struct spi_angle *sa, uint_fast8_t tcode, uint_fast16_t data) { - sa->data[sa->data_count] = tcode; - sa->data[sa->data_count + 1] = data; - sa->data[sa->data_count + 2] = data >> 8; - sa->data_count += 3; + sa->sb.data[sa->sb.data_count] = tcode; + sa->sb.data[sa->sb.data_count + 1] = data; + sa->sb.data[sa->sb.data_count + 2] = data >> 8; + sa->sb.data_count += BYTES_PER_SAMPLE; } // Add an error indicator to the measurement buffer @@ -237,8 +229,7 @@ command_query_spi_angle(uint32_t *args) // Start new measurements query sa->timer.waketime = args[1]; sa->rest_ticks = args[2]; - sa->sequence = 0; - sa->data_count = 0; + sensor_bulk_reset(&sa->sb); sa->time_shift = args[3]; sched_add_timer(&sa->timer); } diff --git a/src/sensor_bulk.c b/src/sensor_bulk.c new file mode 100644 index 00000000..9b5c782c --- /dev/null +++ b/src/sensor_bulk.c @@ -0,0 +1,38 @@ +// Helper code for collecting and sending bulk sensor measurements +// +// Copyright (C) 2020-2023 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "command.h" // sendf +#include "sensor_bulk.h" // sensor_bulk_report + +// Reset counters +void +sensor_bulk_reset(struct sensor_bulk *sb) +{ + sb->sequence = 0; + sb->possible_overflows = 0; + sb->data_count = 0; +} + +// Report local measurement buffer +void +sensor_bulk_report(struct sensor_bulk *sb, uint8_t oid) +{ + sendf("sensor_bulk_data oid=%c sequence=%hu data=%*s" + , oid, sb->sequence, sb->data_count, sb->data); + sb->data_count = 0; + sb->sequence++; +} + +// Report buffer and fifo status +void +sensor_bulk_status(struct sensor_bulk *sb, uint8_t oid + , uint32_t time1, uint32_t query_ticks, uint32_t fifo) +{ + sendf("sensor_bulk_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" + " buffered=%u possible_overflows=%hu" + , oid, time1, query_ticks, sb->sequence + , sb->data_count + fifo, sb->possible_overflows); +} diff --git a/src/sensor_bulk.h b/src/sensor_bulk.h new file mode 100644 index 00000000..9c130bea --- /dev/null +++ b/src/sensor_bulk.h @@ -0,0 +1,15 @@ +#ifndef __SENSOR_BULK_H +#define __SENSOR_BULK_H + +struct sensor_bulk { + uint16_t sequence, possible_overflows; + uint8_t data_count; + uint8_t data[52]; +}; + +void sensor_bulk_reset(struct sensor_bulk *sb); +void sensor_bulk_report(struct sensor_bulk *sb, uint8_t oid); +void sensor_bulk_status(struct sensor_bulk *sb, uint8_t oid + , uint32_t time1, uint32_t query_ticks, uint32_t fifo); + +#endif // sensor_bulk.h diff --git a/src/sensor_lis2dw.c b/src/sensor_lis2dw.c index 06dd3206..579ee1f7 100644 --- a/src/sensor_lis2dw.c +++ b/src/sensor_lis2dw.c @@ -11,6 +11,7 @@ #include "basecmd.h" // oid_alloc #include "command.h" // DECL_COMMAND #include "sched.h" // DECL_TASK +#include "sensor_bulk.h" // sensor_bulk_report #include "spicmds.h" // spidev_transfer #define LIS_AR_DATAX0 0x28 @@ -18,13 +19,14 @@ #define LIS_FIFO_CTRL 0x2E #define LIS_FIFO_SAMPLES 0x2F +#define BYTES_PER_SAMPLE 6 + struct lis2dw { struct timer timer; uint32_t rest_ticks; struct spidev_s *spi; - uint16_t sequence, limit_count; - uint8_t flags, data_count; - uint8_t data[48]; + uint8_t flags; + struct sensor_bulk sb; }; enum { @@ -53,27 +55,6 @@ command_config_lis2dw(uint32_t *args) } DECL_COMMAND(command_config_lis2dw, "config_lis2dw oid=%c spi_oid=%c"); -// Report local measurement buffer -static void -lis2dw_report(struct lis2dw *ax, uint8_t oid) -{ - sendf("lis2dw_data oid=%c sequence=%hu data=%*s" - , oid, ax->sequence, ax->data_count, ax->data); - ax->data_count = 0; - ax->sequence++; -} - -// Report buffer and fifo status -static void -lis2dw_status(struct lis2dw *ax, uint_fast8_t oid - , uint32_t time1, uint32_t time2, uint_fast8_t fifo) -{ - sendf("lis2dw_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" - " buffered=%c fifo=%c limit_count=%hu" - , oid, time1, time2-time1, ax->sequence - , ax->data_count, fifo, ax->limit_count); -} - // Helper code to reschedule the lis2dw_event() timer static void lis2dw_reschedule_timer(struct lis2dw *ax) @@ -93,7 +74,7 @@ lis2dw_query(struct lis2dw *ax, uint8_t oid) uint8_t fifo_empty,fifo_ovrn = 0; msg[0] = LIS_AR_DATAX0 | LIS_AM_READ ; - uint8_t *d = &ax->data[ax->data_count]; + uint8_t *d = &ax->sb.data[ax->sb.data_count]; spidev_transfer(ax->spi, 1, sizeof(msg), msg); @@ -108,13 +89,13 @@ lis2dw_query(struct lis2dw *ax, uint8_t oid) d[4] = msg[5]; // z low bits d[5] = msg[6]; // z high bits - ax->data_count += 6; - if (ax->data_count + 6 > ARRAY_SIZE(ax->data)) - lis2dw_report(ax, oid); + ax->sb.data_count += BYTES_PER_SAMPLE; + if (ax->sb.data_count + BYTES_PER_SAMPLE > ARRAY_SIZE(ax->sb.data)) + sensor_bulk_report(&ax->sb, oid); // Check fifo status if (fifo_ovrn) - ax->limit_count++; + ax->sb.possible_overflows++; // check if we need to run the task again (more packets in fifo?) if (!fifo_empty) { @@ -165,8 +146,7 @@ command_query_lis2dw(uint32_t *args) ax->timer.waketime = args[1]; ax->rest_ticks = args[2]; ax->flags = LIS_HAVE_START; - ax->sequence = ax->limit_count = 0; - ax->data_count = 0; + sensor_bulk_reset(&ax->sb); sched_add_timer(&ax->timer); } DECL_COMMAND(command_query_lis2dw, @@ -180,7 +160,8 @@ command_query_lis2dw_status(uint32_t *args) uint32_t time1 = timer_read_time(); spidev_transfer(ax->spi, 1, sizeof(msg), msg); uint32_t time2 = timer_read_time(); - lis2dw_status(ax, args[0], time1, time2, msg[1]&0x1f); + sensor_bulk_status(&ax->sb, args[0], time1, time2-time1 + , (msg[1] & 0x1f) * BYTES_PER_SAMPLE); } DECL_COMMAND(command_query_lis2dw_status, "query_lis2dw_status oid=%c"); diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c index 7792b4d8..d52de811 100644 --- a/src/sensor_mpu9250.c +++ b/src/sensor_mpu9250.c @@ -12,6 +12,7 @@ #include "basecmd.h" // oid_alloc #include "command.h" // DECL_COMMAND #include "sched.h" // DECL_TASK +#include "sensor_bulk.h" // sensor_bulk_report #include "board/gpio.h" // i2c_read #include "i2ccmds.h" // i2cdev_oid_lookup @@ -46,11 +47,9 @@ struct mpu9250 { struct timer timer; uint32_t rest_ticks; struct i2cdev_s *i2c; - uint16_t sequence, limit_count, fifo_max, fifo_pkts_bytes; - uint8_t flags, data_count; - // msg size must be <= 255 due to Klipper api - // = SAMPLES_PER_BLOCK (from mpu9250.py) * BYTES_PER_FIFO_ENTRY + 1 - uint8_t data[BYTES_PER_BLOCK]; + uint16_t fifo_max, fifo_pkts_bytes; + uint8_t flags; + struct sensor_bulk sb; }; enum { @@ -92,27 +91,6 @@ command_config_mpu9250(uint32_t *args) } DECL_COMMAND(command_config_mpu9250, "config_mpu9250 oid=%c i2c_oid=%c"); -// Report local measurement buffer -static void -mp9250_report(struct mpu9250 *mp, uint8_t oid) -{ - sendf("mpu9250_data oid=%c sequence=%hu data=%*s" - , oid, mp->sequence, mp->data_count, mp->data); - mp->data_count = 0; - mp->sequence++; -} - -// Report buffer and fifo status -static void -mp9250_status(struct mpu9250 *mp, uint_fast8_t oid - , uint32_t time1, uint32_t time2, uint16_t fifo) -{ - sendf("mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu" - " buffered=%c fifo=%u limit_count=%hu" - , oid, time1, time2-time1, mp->sequence - , mp->data_count, fifo, mp->limit_count); -} - // Helper code to reschedule the mpu9250_event() timer static void mp9250_reschedule_timer(struct mpu9250 *mp) @@ -135,10 +113,10 @@ mp9250_query(struct mpu9250 *mp, uint8_t oid) if (mp->fifo_pkts_bytes >= BYTES_PER_BLOCK) { uint8_t reg = AR_FIFO; i2c_read(mp->i2c->i2c_config, sizeof(reg), ® - , BYTES_PER_BLOCK, &mp->data[0]); - mp->data_count = BYTES_PER_BLOCK; + , BYTES_PER_BLOCK, &mp->sb.data[0]); + mp->sb.data_count = BYTES_PER_BLOCK; mp->fifo_pkts_bytes -= BYTES_PER_BLOCK; - mp9250_report(mp, oid); + sensor_bulk_report(&mp->sb, oid); } // If we have enough bytes remaining to fill another report wake again @@ -214,9 +192,7 @@ command_query_mpu9250(uint32_t *args) mp->timer.waketime = args[1]; mp->rest_ticks = args[2]; mp->flags = AX_HAVE_START; - mp->sequence = 0; - mp->limit_count = 0; - mp->data_count = 0; + sensor_bulk_reset(&mp->sb); mp->fifo_max = 0; mp->fifo_pkts_bytes = 0; sched_add_timer(&mp->timer); @@ -235,7 +211,7 @@ command_query_mpu9250_status(uint32_t *args) i2c_read(mp->i2c->i2c_config, sizeof(int_reg), int_reg, sizeof(int_msg), &int_msg); if (int_msg & FIFO_OVERFLOW_INT) - mp->limit_count++; + mp->sb.possible_overflows++; // Read latest FIFO count (with precise timing) uint8_t reg[] = {AR_FIFO_COUNT_H}; @@ -246,7 +222,7 @@ command_query_mpu9250_status(uint32_t *args) uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1]; // Report status - mp9250_status(mp, args[0], time1, time2, fifo_bytes / BYTES_PER_FIFO_ENTRY); + sensor_bulk_status(&mp->sb, args[0], time1, time2-time1, fifo_bytes); } DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c");