Add support for ICM20948 accelerometer #6756
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
|
There was a problem hiding this comment. there are There was a problem hiding this comment. I'll swap that in. Thanks for the suggestion @nefelim4ag |
||
| 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], | ||
|
There was a problem hiding this comment. Those will reorder writes, cause your next set_reg will execute with minclock=0 BTW, you should be able to just write 2 bytes at address of DIV1 There was a problem hiding this comment. This is ultimately duplicated from klippy/extras/mpu9250.py. I can change icm20948.py to import functions from there where they're unchanged. There was a problem hiding this comment. Yes, I already found that, but this is your code now :D With wait_ack() you can just add a pause there, before write, or order other writes by minclock There was a problem hiding this comment. According to the datasheet, there is a 20ms delay between the chip wake and accelerometer ready (how ready, what does it mean?) but looks like all registers can be just written There was a problem hiding this comment.
Commands are never reordered within a cmdqueue (and all the i2c commands will be on the same cmdqueue). The There are a few ways to enforce a delay between i2c writes. One way is to do what mpu9250 does: This forces an i2c read so that we know previous commands have been completed at the micro-controller (that is, nothing in the command queue). Then we schedule the next i2c message to have a minimum clock that is 20ms in the future (from the current time as measured on the host). Note that the Another mechanism would be something like: This differs from the above in that we pause the host instead of just scheduling the next message at a particular time. (Both will enforce at least 20ms of delay between i2c messages.) As mentioned, it's possible to eliminate the dummy read by using Of course, if no delay is actually needed, that is preferable. Hope that helps, |
||
| 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) | ||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
oidis not used anywhere