icm20948: Add support for ICM20948 accelerometer (#6756)

Signed-off-by: Paul Hansel <github@paulhansel.com>

5 files changed, 371 insertions, 5 deletions

diff --git a/docs/Measuring_Resonances.md b/docs/Measuring_Resonances.md
index d6aaf567..e4f9b8e9 100644
--- a/docs/Measuring_Resonances.md
+++ b/docs/Measuring_Resonances.md
@@ -18,9 +18,9 @@ board designs and different clones of them. If it is going to be connected to a
 For ADXL345s, make sure that the board supports SPI mode (a small number of
 boards appear to be hard-configured for I2C by pulling SDO to GND).
 
-For MPU-9250/MPU-9255/MPU-6515/MPU-6050/MPU-6500s and LIS2DW/LIS3DH there are also
-a variety of board designs and clones with different I2C pull-up resistors which
-will need supplementing.
+For MPU-9250/MPU-9255/MPU-6515/MPU-6050/MPU-6500/ICM20948s and LIS2DW/LIS3DH there
+are also a variety of board designs and clones with different I2C pull-up resistors
+which will need supplementing.
 
 ## MCUs with Klipper I2C *fast-mode* Support
 
@@ -136,7 +136,7 @@ GND+SCL
 
 Note that unlike a cable shield, any GND(s) should be connected at both ends.
 
-#### MPU-9250/MPU-9255/MPU-6515/MPU-6050/MPU-6500
+#### MPU-9250/MPU-9255/MPU-6515/MPU-6050/MPU-6500/ICM20948
 
 These accelerometers have been tested to work over I2C on the RPi, RP2040 (Pico)
 and AVR at 400kbit/s (*fast mode*). Some MPU accelerometer modules include
@@ -355,6 +355,7 @@ accel_chip: mpu9250
 probe_points:
     100, 100, 20  # an example
 ```
+If you are using the ICM20948, replace instances of "mpu9250" with "icm20948".
 
 #### Configure MPU-9520 Compatibles With Pico
 
@@ -377,6 +378,7 @@ probe_points:
 [static_digital_output pico_3V3pwm] # Improve power stability
 pins: pico:gpio23
 ```
+If you are using the ICM20948, replace instances of "mpu9250" with "icm20948".
 
 #### Configure MPU-9520 Compatibles with AVR
 
@@ -395,6 +397,7 @@ accel_chip: mpu9250
 probe_points:
     100, 100, 20  # an example
 ```
+If you are using the ICM20948, replace instances of "mpu9250" with "icm20948".
 
 Restart Klipper via the `RESTART` command.
 
diff --git a/klippy/extras/icm20948.py b/klippy/extras/icm20948.py
new file mode 100644
index 00000000..1aafdd98
--- /dev/null
+++ b/klippy/extras/icm20948.py
@@ -0,0 +1,171 @@
+# Support for reading acceleration data from an icm20948 chip
+#
+# Copyright (C) 2024 Paul Hansel <github@paulhansel.com>
+# Copyright (C) 2022  Harry Beyel <harry3b9@gmail.com>
+# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+# From https://invensense.tdk.com/wp-content/uploads/
+#               2016/06/DS-000189-ICM-20948-v1.3.pdf
+
+import logging
+from . import bus, adxl345, bulk_sensor
+
+ICM20948_ADDR =      0x68
+
+ICM_DEV_IDS = {
+    0xEA: "icm-20948",
+    #everything above are normal ICM IDs
+    }
+
+
+# ICM20948 registers
+REG_DEVID =         0x00 # 0xEA
+REG_FIFO_EN =       0x67 # FIFO_EN_2
+REG_ACCEL_SMPLRT_DIV1 =    0x10 # MSB
+REG_ACCEL_SMPLRT_DIV2 =    0x11 # LSB
+REG_ACCEL_CONFIG    =    0x14
+REG_USER_CTRL =     0x03
+REG_PWR_MGMT_1 = 0x06
+REG_PWR_MGMT_2 = 0x07
+REG_INT_STATUS = 0x19
+
+SAMPLE_RATE_DIVS = { 4500: 0x00 }
+
+#SET_CONFIG =        0x01 # FIFO mode 'stream' style
+SET_ACCEL_CONFIG =  0x04 # 8g full scale, 1209Hz BW, ??? delay 4.5kHz samp rate
+SET_PWR_MGMT_1_WAKE =     0x01
+SET_PWR_MGMT_1_SLEEP=     0x41
+SET_PWR_MGMT_2_ACCEL_ON = 0x07
+SET_PWR_MGMT_2_OFF  =     0x3F
+SET_USER_FIFO_RESET = 0x0E
+SET_USER_FIFO_EN    = 0x40
+SET_ENABLE_FIFO  = 0x10
+SET_DISABLE_FIFO = 0x00
+
+FREEFALL_ACCEL = 9.80665 * 1000.
+# SCALE = 1/4096 g/LSB @8g scale * Earth gravity in mm/s**2
+SCALE = 0.000244140625 * FREEFALL_ACCEL
+
+FIFO_SIZE = 512
+
+BATCH_UPDATES = 0.100
+
+# Printer class that controls ICM20948 chip
+class ICM20948:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        adxl345.AccelCommandHelper(config, self)
+        self.axes_map = adxl345.read_axes_map(config, SCALE, SCALE, SCALE)
+        self.data_rate = config.getint('rate', 4500)
+        if self.data_rate not in SAMPLE_RATE_DIVS:
+            raise config.error("Invalid rate parameter: %d" % (self.data_rate,))
+        # Setup mcu sensor_icm20948 bulk query code
+        self.i2c = bus.MCU_I2C_from_config(config,
+                                           default_addr=ICM20948_ADDR,
+                                           default_speed=400000)
+        self.mcu = mcu = self.i2c.get_mcu()
+        self.oid = mcu.create_oid()
+        self.query_icm20948_cmd = None
+        mcu.register_config_callback(self._build_config)
+        # Bulk sample message reading
+        chip_smooth = self.data_rate * BATCH_UPDATES * 2
+        self.ffreader = bulk_sensor.FixedFreqReader(mcu, chip_smooth, ">hhh")
+        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]
+        hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration')
+        self.batch_bulk.add_mux_endpoint("icm20948/dump_icm20948", "sensor",
+                                         self.name, {'header': hdr})
+    def _build_config(self):
+        cmdqueue = self.i2c.get_command_queue()
+        self.mcu.add_config_cmd("config_icm20948 oid=%d i2c_oid=%d"
+                           % (self.oid, self.i2c.get_oid()))
+        self.mcu.add_config_cmd("query_icm20948 oid=%d rest_ticks=0"
+                           % (self.oid,), on_restart=True)
+        self.query_icm20948_cmd = self.mcu.lookup_command(
+            "query_icm20948 oid=%c rest_ticks=%u", cq=cmdqueue)
+        self.ffreader.setup_query_command("query_icm20948_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]
+    def set_reg(self, reg, val, minclock=0):
+        self.i2c.i2c_write([reg, val & 0xFF], minclock=minclock)
+    def start_internal_client(self):
+        aqh = adxl345.AccelQueryHelper(self.printer)
+        self.batch_bulk.add_client(aqh.handle_batch)
+        return aqh
+    # Measurement decoding
+    def _convert_samples(self, samples):
+        (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
+        count = 0
+        for ptime, rx, ry, rz in samples:
+            raw_xyz = (rx, ry, rz)
+            x = round(raw_xyz[x_pos] * x_scale, 6)
+            y = round(raw_xyz[y_pos] * y_scale, 6)
+            z = round(raw_xyz[z_pos] * z_scale, 6)
+            samples[count] = (round(ptime, 6), x, y, z)
+            count += 1
+    # Start, stop, and process message batches
+    def _start_measurements(self):
+        # In case of miswiring, testing ICM20948 device ID prevents treating
+        # noise or wrong signal as a correctly initialized device
+        dev_id = self.read_reg(REG_DEVID)
+        if dev_id not in ICM_DEV_IDS.keys():
+            raise self.printer.command_error(
+                "Invalid mpu id (got %x).\n"
+                "This is generally indicative of connection problems\n"
+                "(e.g. faulty wiring) or a faulty chip."
+                % (dev_id))
+        else:
+            logging.info("Found %s with id %x"% (ICM_DEV_IDS[dev_id], dev_id))
+
+        # Setup chip in requested query rate
+        self.set_reg(REG_PWR_MGMT_1, SET_PWR_MGMT_1_WAKE)
+        self.set_reg(REG_PWR_MGMT_2, SET_PWR_MGMT_2_ACCEL_ON)
+        # Don't add 20ms pause for accelerometer chip wake up
+        self.read_reg(REG_DEVID) # Dummy read to ensure queues flushed
+        self.set_reg(REG_ACCEL_SMPLRT_DIV1, SAMPLE_RATE_DIVS[self.data_rate])
+        self.set_reg(REG_ACCEL_SMPLRT_DIV2, SAMPLE_RATE_DIVS[self.data_rate])
+        # self.set_reg(REG_CONFIG, SET_CONFIG) # No config register
+        self.set_reg(REG_ACCEL_CONFIG, SET_ACCEL_CONFIG)
+        # Reset fifo
+        self.set_reg(REG_FIFO_EN, SET_DISABLE_FIFO)
+        self.set_reg(REG_USER_CTRL, SET_USER_FIFO_RESET)
+        self.set_reg(REG_USER_CTRL, SET_USER_FIFO_EN)
+        self.read_reg(REG_INT_STATUS) # clear FIFO overflow flag
+
+        # Start bulk reading
+        rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
+        self.query_icm20948_cmd.send([self.oid, rest_ticks])
+        self.set_reg(REG_FIFO_EN, SET_ENABLE_FIFO)
+        logging.info("ICM20948 starting '%s' measurements", self.name)
+        # Initialize clock tracking
+        self.ffreader.note_start()
+        self.last_error_count = 0
+    def _finish_measurements(self):
+        # Halt bulk reading
+        self.set_reg(REG_FIFO_EN, SET_DISABLE_FIFO)
+        self.query_icm20948_cmd.send_wait_ack([self.oid, 0])
+        self.ffreader.note_end()
+        logging.info("ICM20948 finished '%s' measurements", self.name)
+        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):
+        samples = self.ffreader.pull_samples()
+        self._convert_samples(samples)
+        if not samples:
+            return {}
+        return {'data': samples, 'errors': self.last_error_count,
+                'overflows': self.ffreader.get_last_overflows()}
+
+def load_config(config):
+    return ICM20948(config)
+
+def load_config_prefix(config):
+    return ICM20948(config)
diff --git a/src/Kconfig b/src/Kconfig
index 16857272..9923ed7f 100644
--- a/src/Kconfig
+++ b/src/Kconfig
@@ -112,6 +112,10 @@ config WANT_MPU9250
     bool
     depends on HAVE_GPIO_I2C
     default y
+config WANT_ICM20948
+    bool
+    depends on HAVE_GPIO_I2C
+    default y
 config WANT_HX71X
     bool
     depends on WANT_GPIO_BITBANGING
@@ -138,7 +142,7 @@ config WANT_SOFTWARE_SPI
     default y
 config NEED_SENSOR_BULK
     bool
-    depends on WANT_ADXL345 || WANT_LIS2DW || WANT_MPU9250 \
+    depends on WANT_ADXL345 || WANT_LIS2DW || WANT_MPU9250 || WANT_ICM20948 \
         || WANT_HX71X || WANT_ADS1220 || WANT_LDC1612 || WANT_SENSOR_ANGLE
     default y
 menu "Optional features (to reduce code size)"
@@ -161,6 +165,9 @@ config WANT_LIS2DW
 config WANT_MPU9250
     bool "Support MPU accelerometers"
     depends on HAVE_GPIO_I2C
+config WANT_ICM20948
+    bool "Support ICM20948 accelerometer"
+    depends on HAVE_GPIO_I2C
 config WANT_HX71X
     bool "Support HX711 and HX717 ADC chips"
     depends on WANT_GPIO_BITBANGING
diff --git a/src/Makefile b/src/Makefile
index a0ca02b9..a7343ffc 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -18,6 +18,7 @@ src-$(CONFIG_WANT_THERMOCOUPLE) += thermocouple.c
 src-$(CONFIG_WANT_ADXL345) += sensor_adxl345.c
 src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c
 src-$(CONFIG_WANT_MPU9250) += sensor_mpu9250.c
+src-$(CONFIG_WANT_ICM20948) += sensor_icm20948.c
 src-$(CONFIG_WANT_HX71X) += sensor_hx71x.c
 src-$(CONFIG_WANT_ADS1220) += sensor_ads1220.c
 src-$(CONFIG_WANT_LDC1612) += sensor_ldc1612.c
diff --git a/src/sensor_icm20948.c b/src/sensor_icm20948.c
new file mode 100644
index 00000000..eb73a52c
--- /dev/null
+++ b/src/sensor_icm20948.c
@@ -0,0 +1,184 @@
+// Support for gathering acceleration data from icm20948 chip
+//
+// Copyright (C) 2024 Paul Hansel <github@paulhansel.com>
+// Copyright (C) 2023 Matthew Swabey <matthew@swabey.org>
+// Copyright (C) 2022 Harry Beyel <harry3b9@gmail.com>
+// Copyright (C) 2020-2023 Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <string.h> // memcpy
+#include "board/irq.h" // irq_disable
+#include "board/misc.h" // timer_read_time
+#include "basecmd.h" // oid_alloc
+#include "command.h" // DECL_COMMAND
+#include "sched.h" // DECL_TASK
+#include "sensor_bulk.h" // sensor_bulk_report
+#include "i2ccmds.h" // i2cdev_oid_lookup
+
+// Chip registers
+#define AR_FIFO_COUNT_H 0x70
+#define AR_FIFO         0x72
+#define AR_INT_STATUS   0x1B // INT_STATUS_2
+
+#define FIFO_OVERFLOW_INT 0x0F // from datasheet
+
+#define BYTES_PER_FIFO_ENTRY 6
+#define BYTES_PER_BLOCK 48
+
+struct icm20948 {
+    struct timer timer;
+    uint32_t rest_ticks;
+    struct i2cdev_s *i2c;
+    uint16_t fifo_max, fifo_pkts_bytes;
+    uint8_t flags;
+    struct sensor_bulk sb;
+};
+
+enum {
+    AX_PENDING = 1<<0,
+};
+
+static struct task_wake icm20948_wake;
+
+// Event handler that wakes icm20948_task() periodically
+static uint_fast8_t
+icm20948_event(struct timer *timer)
+{
+    struct icm20948 *ax = container_of(timer, struct icm20948, timer);
+    ax->flags |= AX_PENDING;
+    sched_wake_task(&icm20948_wake);
+    return SF_DONE;
+}
+
+void
+command_config_icm20948(uint32_t *args)
+{
+    struct icm20948 *ic = oid_alloc(args[0], command_config_icm20948
+                                   , sizeof(*ic));
+    ic->timer.func = icm20948_event;
+    ic->i2c = i2cdev_oid_lookup(args[1]);
+}
+DECL_COMMAND(command_config_icm20948, "config_icm20948 oid=%c i2c_oid=%c");
+
+// Helper code to reschedule the icm20948_event() timer
+static void
+ic20948_reschedule_timer(struct icm20948 *ic)
+{
+    irq_disable();
+    ic->timer.waketime = timer_read_time() + ic->rest_ticks;
+    sched_add_timer(&ic->timer);
+    irq_enable();
+}
+
+static void
+read_mpu(struct i2cdev_s *i2c, uint8_t reg_len, uint8_t *reg
+         , uint8_t read_len, uint8_t *read)
+{
+    int ret = i2c_dev_read(i2c, reg_len, reg, read_len, read);
+    i2c_shutdown_on_err(ret);
+}
+
+// Reads the fifo byte count from the device.
+static uint16_t
+get_fifo_status(struct icm20948 *ic)
+{
+    uint8_t reg[] = {AR_FIFO_COUNT_H};
+    uint8_t msg[2];
+    read_mpu(ic->i2c, sizeof(reg), reg, sizeof(msg), msg);
+    uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
+    if (fifo_bytes > ic->fifo_max)
+        ic->fifo_max = fifo_bytes;
+    return fifo_bytes;
+}
+
+// Query accelerometer data
+static void
+ic20948_query(struct icm20948 *ic, uint8_t oid)
+{
+    // If not enough bytes to fill report read MPU FIFO's fill
+    if (ic->fifo_pkts_bytes < BYTES_PER_BLOCK)
+        ic->fifo_pkts_bytes = get_fifo_status(ic);
+
+    // If we have enough bytes to fill the buffer do it and send report
+    if (ic->fifo_pkts_bytes >= BYTES_PER_BLOCK) {
+        uint8_t reg = AR_FIFO;
+        read_mpu(ic->i2c, sizeof(reg), &reg, BYTES_PER_BLOCK, &ic->sb.data[0]);
+        ic->sb.data_count = BYTES_PER_BLOCK;
+        ic->fifo_pkts_bytes -= BYTES_PER_BLOCK;
+        sensor_bulk_report(&ic->sb, oid);
+    }
+
+    // If we have enough bytes remaining to fill another report wake again
+    //  otherwise schedule timed wakeup
+    if (ic->fifo_pkts_bytes >= BYTES_PER_BLOCK) {
+        sched_wake_task(&icm20948_wake);
+    } else {
+        ic->flags &= ~AX_PENDING;
+        ic20948_reschedule_timer(ic);
+    }
+}
+
+void
+command_query_icm20948(uint32_t *args)
+{
+    struct icm20948 *ic = oid_lookup(args[0], command_config_icm20948);
+
+    sched_del_timer(&ic->timer);
+    ic->flags = 0;
+    if (!args[1]) {
+        // End measurements
+
+        // Uncomment and rebuilt to check for FIFO overruns when tuning
+        //output("mpu9240 fifo_max=%u", ic->fifo_max);
+        return;
+    }
+
+    // Start new measurements query
+    ic->rest_ticks = args[1];
+    sensor_bulk_reset(&ic->sb);
+    ic->fifo_max = 0;
+    ic->fifo_pkts_bytes = 0;
+    ic20948_reschedule_timer(ic);
+}
+DECL_COMMAND(command_query_icm20948, "query_icm20948 oid=%c rest_ticks=%u");
+
+void
+command_query_icm20948_status(uint32_t *args)
+{
+    struct icm20948 *ic = oid_lookup(args[0], command_config_icm20948);
+
+    // Detect if a FIFO overrun occurred
+    uint8_t int_reg[] = {AR_INT_STATUS};
+    uint8_t int_msg;
+    read_mpu(ic->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), &int_msg);
+    if (int_msg & FIFO_OVERFLOW_INT)
+        ic->sb.possible_overflows++;
+
+    // Read latest FIFO count (with precise timing)
+    uint8_t reg[] = {AR_FIFO_COUNT_H};
+    uint8_t msg[2];
+    uint32_t time1 = timer_read_time();
+    read_mpu(ic->i2c, sizeof(reg), reg, sizeof(msg), msg);
+    uint32_t time2 = timer_read_time();
+    uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
+
+    // Report status
+    sensor_bulk_status(&ic->sb, args[0], time1, time2-time1, fifo_bytes);
+}
+DECL_COMMAND(command_query_icm20948_status, "query_icm20948_status oid=%c");
+
+void
+icm20948_task(void)
+{
+    if (!sched_check_wake(&icm20948_wake))
+        return;
+    uint8_t oid;
+    struct icm20948 *ic;
+    foreach_oid(oid, ic, command_config_icm20948) {
+        uint_fast8_t flags = ic->flags;
+        if (flags & AX_PENDING)
+            ic20948_query(ic, oid);
+    }
+}
+DECL_TASK(icm20948_task);