Add support for ICM20948 accelerometer

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.
 

klippy/extras/icm20948.py

@@ -0,0 +1,175 @@
+# Support for reading acceleration data from an icm20948 chip
+#
+# Copyright (C) 2024 Paul Hansel <[email protected]>
+# Copyright (C) 2022  Harry Beyel <[email protected]>
+# Copyright (C) 2020-2021 Kevin O'Connor <[email protected]>
+#
+# 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 = 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)
+        # Add 20ms pause for accelerometer chip wake up
+        self.read_reg(REG_DEVID) # Dummy read to ensure queues flushed
+        systime = self.printer.get_reactor().monotonic()
+        next_time = self.mcu.estimated_print_time(systime) + 0.020
+        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],
+                     minclock=self.mcu.print_time_to_clock(next_time))
+        # self.set_reg(REG_CONFIG, SET_CONFIG) # No config register
+        self.set_reg(REG_ACCEL_CONFIG, SET_ACCEL_CONFIG)
+        # self.set_reg(REG_ACCEL_CONFIG2, SET_ACCEL_CONFIG2)
+        # 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)

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

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