diff --git a/klippy/extras/lis2dw.py b/klippy/extras/lis2dw.py index 469a4f0d..e9631302 100644 --- a/klippy/extras/lis2dw.py +++ b/klippy/extras/lis2dw.py @@ -1,7 +1,7 @@ # Support for reading acceleration data from an LIS2DW chip # # Copyright (C) 2023 Zhou.XianMing -# Copyright (C) 2020-2021 Kevin O'Connor +# Copyright (C) 2020-2023 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import logging @@ -30,8 +30,6 @@ LIS2DW_DEV_ID = 0x44 FREEFALL_ACCEL = 9.80665 SCALE = FREEFALL_ACCEL * 1.952 / 4 -MIN_MSG_TIME = 0.100 - BYTES_PER_SAMPLE = 6 SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE @@ -51,7 +49,7 @@ class LIS2DW: self.query_lis2dw_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" + mcu.add_config_cmd("query_lis2dw oid=%d rest_ticks=0" % (oid,), on_restart=True) mcu.register_config_callback(self._build_config) self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid) @@ -73,7 +71,7 @@ class LIS2DW: def _build_config(self): 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) + "query_lis2dw oid=%c rest_ticks=%u", 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): @@ -154,19 +152,17 @@ class LIS2DW: # 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_lis2dw_cmd.send([self.oid, reqclock, rest_ticks], - reqclock=reqclock) + self.query_lis2dw_cmd.send([self.oid, rest_ticks]) + self.set_reg(REG_LIS2DW_FIFO_CTRL, 0xC0) logging.info("LIS2DW starting '%s' measurements", self.name) # Initialize clock tracking self.clock_updater.note_start() self.last_error_count = 0 def _finish_measurements(self): # Halt bulk reading - self.query_lis2dw_cmd.send_wait_ack([self.oid, 0, 0]) + self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00) + self.query_lis2dw_cmd.send_wait_ack([self.oid, 0]) self.bulk_queue.clear_samples() logging.info("LIS2DW finished '%s' measurements", self.name) self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00) diff --git a/src/sensor_lis2dw.c b/src/sensor_lis2dw.c index 579ee1f7..83922003 100644 --- a/src/sensor_lis2dw.c +++ b/src/sensor_lis2dw.c @@ -1,7 +1,7 @@ // Support for gathering acceleration data from LIS2DW chip // // Copyright (C) 2023 Zhou.XianMing -// Copyright (C) 2020 Kevin O'Connor +// Copyright (C) 2020-2023 Kevin O'Connor // // This file may be distributed under the terms of the GNU GPLv3 license. @@ -16,7 +16,6 @@ #define LIS_AR_DATAX0 0x28 #define LIS_AM_READ 0x80 -#define LIS_FIFO_CTRL 0x2E #define LIS_FIFO_SAMPLES 0x2F #define BYTES_PER_SAMPLE 6 @@ -30,7 +29,7 @@ struct lis2dw { }; enum { - LIS_HAVE_START = 1<<0, LIS_RUNNING = 1<<1, LIS_PENDING = 1<<2, + LIS_PENDING = 1<<0, }; static struct task_wake lis2dw_wake; @@ -101,56 +100,30 @@ lis2dw_query(struct lis2dw *ax, uint8_t oid) if (!fifo_empty) { // More data in fifo - wake this task again sched_wake_task(&lis2dw_wake); - } else if (ax->flags & LIS_RUNNING) { + } else { // Sleep until next check time - sched_del_timer(&ax->timer); ax->flags &= ~LIS_PENDING; lis2dw_reschedule_timer(ax); } } -// Startup measurements -static void -lis2dw_start(struct lis2dw *ax, uint8_t oid) -{ - sched_del_timer(&ax->timer); - ax->flags = LIS_RUNNING; - uint8_t ctrl[2] = {LIS_FIFO_CTRL , 0xC0}; - spidev_transfer(ax->spi, 0, sizeof(ctrl), ctrl); - lis2dw_reschedule_timer(ax); -} - -// End measurements -static void -lis2dw_stop(struct lis2dw *ax, uint8_t oid) -{ - // Disable measurements - sched_del_timer(&ax->timer); - ax->flags = 0; - uint8_t ctrl[2] = {LIS_FIFO_CTRL , 0}; - spidev_transfer(ax->spi, 0, sizeof(ctrl), ctrl); -} - void command_query_lis2dw(uint32_t *args) { struct lis2dw *ax = oid_lookup(args[0], command_config_lis2dw); - if (!args[2]) { - // End measurements - lis2dw_stop(ax, args[0]); - return; - } - // Start new measurements query sched_del_timer(&ax->timer); - ax->timer.waketime = args[1]; - ax->rest_ticks = args[2]; - ax->flags = LIS_HAVE_START; + ax->flags = 0; + if (!args[1]) + // End measurements + return; + + // Start new measurements query + ax->rest_ticks = args[1]; sensor_bulk_reset(&ax->sb); - sched_add_timer(&ax->timer); + lis2dw_reschedule_timer(ax); } -DECL_COMMAND(command_query_lis2dw, - "query_lis2dw oid=%c clock=%u rest_ticks=%u"); +DECL_COMMAND(command_query_lis2dw, "query_lis2dw oid=%c rest_ticks=%u"); void command_query_lis2dw_status(uint32_t *args) @@ -174,11 +147,7 @@ lis2dw_task(void) struct lis2dw *ax; foreach_oid(oid, ax, command_config_lis2dw) { uint_fast8_t flags = ax->flags; - if (!(flags & LIS_PENDING)) - continue; - if (flags & LIS_HAVE_START) - lis2dw_start(ax, oid); - else + if (flags & LIS_PENDING) lis2dw_query(ax, oid); } }