diff --git a/klippy/extras/icm20948.py b/klippy/extras/icm20948.py new file mode 100644 index 000000000000..407e8501c38d --- /dev/null +++ b/klippy/extras/icm20948.py @@ -0,0 +1,175 @@ +# Support for reading acceleration data from an icm20948 chip +# +# Copyright (C) 2024 Paul Hansel +# Copyright (C) 2022 Harry Beyel +# Copyright (C) 2020-2021 Kevin O'Connor +# +# 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) diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py index d305cff26430..ff15fed41be6 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -16,14 +16,10 @@ 0x70: "mpu-6500", 0x68: "mpu-6050", #everything above are normal MPU IDs - 0xEA: "icm-20948", 0x75: "mpu-unknown (DEFECTIVE! USE WITH CAUTION!)", 0x69: "mpu-unknown (DEFECTIVE! USE WITH CAUTION!)", } -REG_DEVID_ICM20948 = 0x00 -ICM_DEV_ID = 0xEA - # MPU9250 registers REG_DEVID = 0x75 REG_FIFO_EN = 0x23 @@ -65,7 +61,6 @@ def __init__(self, config): adxl345.AccelCommandHelper(config, self) self.axes_map = adxl345.read_axes_map(config, SCALE, SCALE, SCALE) self.data_rate = config.getint('rate', 4000) - self.this_mpu9250_is_icm20948 = False if self.data_rate not in SAMPLE_RATE_DIVS: raise config.error("Invalid rate parameter: %d" % (self.data_rate,)) # Setup mcu sensor_mpu9250 bulk query code @@ -74,10 +69,8 @@ def __init__(self, config): default_speed=400000) self.mcu = mcu = self.i2c.get_mcu() self.oid = oid = mcu.create_oid() - self.query_mpu9250_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") @@ -90,23 +83,6 @@ def __init__(self, config): hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration') self.batch_bulk.add_mux_endpoint("mpu9250/dump_mpu9250", "sensor", self.name, {'header': hdr}) - dev_id_icm = self.read_reg(REG_DEVID_ICM20948) - if dev_id_icm == ICM_DEV_ID: - self.this_mpu9250_is_icm20948 = True - self._exchange_registers_icm20948() - self.data_rate = config.getint('rate', 4500) - 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("mpu9250/dump_mpu9250", "sensor", - self.name, {'header': hdr}) - def _build_config(self): cmdqueue = self.i2c.get_command_queue() self.mcu.add_config_cmd("config_mpu9250 oid=%d i2c_oid=%d" @@ -117,7 +93,6 @@ def _build_config(self): "query_mpu9250 oid=%c rest_ticks=%u", cq=cmdqueue) self.ffreader.setup_query_command("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] @@ -143,9 +118,6 @@ def _start_measurements(self): # In case of miswiring, testing MPU9250 device ID prevents treating # noise or wrong signal as a correctly initialized device dev_id = self.read_reg(REG_DEVID) - icm_dev_id = self.read_reg(REG_DEVID_ICM20948) - if icm_dev_id in MPU_DEV_IDS.keys(): - dev_id = icm_dev_id if dev_id not in MPU_DEV_IDS.keys(): raise self.printer.command_error( "Invalid mpu id (got %x).\n" @@ -154,7 +126,6 @@ def _start_measurements(self): % (dev_id)) else: logging.info("Found %s with id %x"% (MPU_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) @@ -162,20 +133,11 @@ def _start_measurements(self): 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 - if not self.this_mpu9250_is_icm20948: - self.set_reg(REG_SMPLRT_DIV, SAMPLE_RATE_DIVS[self.data_rate], - minclock=self.mcu.print_time_to_clock(next_time)) - else: - 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)) - if not self.this_mpu9250_is_icm20948: - self.set_reg(REG_CONFIG, SET_CONFIG) + self.set_reg(REG_SMPLRT_DIV, SAMPLE_RATE_DIVS[self.data_rate], + minclock=self.mcu.print_time_to_clock(next_time)) + self.set_reg(REG_CONFIG, SET_CONFIG) self.set_reg(REG_ACCEL_CONFIG, SET_ACCEL_CONFIG) - if not self.this_mpu9250_is_icm20948: - self.set_reg(REG_ACCEL_CONFIG2, SET_ACCEL_CONFIG2) + 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) @@ -206,54 +168,6 @@ def _process_batch(self, eventtime): return {'data': samples, 'errors': self.last_error_count, 'overflows': self.ffreader.get_last_overflows()} - def _exchange_registers_icm20948(self): - # Exchanges the register addresses and values - # in this file from MPU9250-compat to - # ICM20948 compat. - global REG_DEVID - REG_DEVID = 0x00 # 0xEA - global REG_FIFO_EN - REG_FIFO_EN = 0x67 # FIFO_EN_2 - global REG_ACCEL_SMPLRT_DIV1 - REG_ACCEL_SMPLRT_DIV1 = 0x10 # MSB - global REG_ACCEL_SMPLRT_DIV2 - REG_ACCEL_SMPLRT_DIV2 = 0x11 # LSB - global REG_ACCEL_CONFIG - REG_ACCEL_CONFIG = 0x14 - global REG_USER_CTRL - REG_USER_CTRL = 0x03 - global REG_PWR_MGMT_1 - REG_PWR_MGMT_1 = 0x06 - global REG_PWR_MGMT_2 - REG_PWR_MGMT_2 = 0x07 - global REG_INT_STATUS - REG_INT_STATUS = 0x19 - - global SAMPLE_RATE_DIVS - SAMPLE_RATE_DIVS = { 4000: 0x00, 4500: 0x00 } - - #SET_CONFIG = 0x01 # FIFO mode 'stream' style - global SET_ACCEL_CONFIG - SET_ACCEL_CONFIG = 0x04 # 8g full scale, 1209Hz BW - # ??? delay, 4.5kHz samp rate - global SET_PWR_MGMT_1_WAKE - SET_PWR_MGMT_1_WAKE = 0x01 - global SET_PWR_MGMT_1_SLEEP - SET_PWR_MGMT_1_SLEEP = 0x41 - global SET_PWR_MGMT_2_ACCEL_ON - SET_PWR_MGMT_2_ACCEL_ON = 0x07 - global SET_PWR_MGMT_2_OFF - SET_PWR_MGMT_2_OFF = 0x3F - global SET_USER_FIFO_RESET - SET_USER_FIFO_RESET = 0x0E - global SET_USER_FIFO_EN - SET_USER_FIFO_EN = 0x40 - global SET_ENABLE_FIFO - SET_ENABLE_FIFO = 0x10 - global SET_DISABLE_FIFO - SET_DISABLE_FIFO = 0x00 - - def load_config(config): return MPU9250(config) diff --git a/src/sensor_icm20948.c b/src/sensor_icm20948.c new file mode 100644 index 000000000000..eb73a52c15b1 --- /dev/null +++ b/src/sensor_icm20948.c @@ -0,0 +1,184 @@ +// Support for gathering acceleration data from icm20948 chip +// +// Copyright (C) 2024 Paul Hansel +// Copyright (C) 2023 Matthew Swabey +// Copyright (C) 2022 Harry Beyel +// Copyright (C) 2020-2023 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // 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), ®, 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); diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c index fa8fa51ec863..7476734c4f0a 100644 --- a/src/sensor_mpu9250.c +++ b/src/sensor_mpu9250.c @@ -22,24 +22,9 @@ #define FIFO_OVERFLOW_INT 0x10 -#define ICM_AR_FIFO_COUNT_H 0x70 -#define ICM_AR_FIFO 0x72 -#define ICM_AR_INT_STATUS 0x1B // INT_STATUS_2 - -#define ICM_FIFO_OVERFLOW_INT 0x0F // from datasheet - -#define ICM_REG_ID 0x00 -#define ICM_DEV_ID 0xEA - #define BYTES_PER_FIFO_ENTRY 6 #define BYTES_PER_BLOCK 48 -enum mp_type{ - ICM, // icm20947 - MPU, // mpu9249 type compatible - UNINITMPU // pre-init -}; - struct mpu9250 { struct timer timer; uint32_t rest_ticks; @@ -47,7 +32,6 @@ struct mpu9250 { uint16_t fifo_max, fifo_pkts_bytes; uint8_t flags; struct sensor_bulk sb; - enum mp_type variant; }; enum { @@ -56,30 +40,11 @@ enum { static struct task_wake mpu9250_wake; -static void -autodetect_mpu_icm(struct mpu9250 *mp) -{ - // determine whether this sensor is an MPU9250-like - // or ICM20948 by reading ICM_REG_ID. - if (mp->variant == UNINITMPU) { - uint8_t reg[] = {ICM_REG_ID}; - uint8_t msg[1]; - read_mpu(mp->i2c, 1, reg, 1, msg); - if (msg[0] == ICM_DEV_ID) { - mp->variant = ICM; - } else { - // register 0x00 will be 0x00 on reset for MPU9250 - mp->variant = MPU; - } - } -} - // Event handler that wakes mpu9250_task() periodically static uint_fast8_t mpu9250_event(struct timer *timer) { struct mpu9250 *ax = container_of(timer, struct mpu9250, timer); - autodetect_mpu_icm(ax); ax->flags |= AX_PENDING; sched_wake_task(&mpu9250_wake); return SF_DONE; @@ -88,9 +53,8 @@ mpu9250_event(struct timer *timer) void command_config_mpu9250(uint32_t *args) { - struct mpu9250 *mp = oid_alloc(args[0], command_config_mpu9250, - sizeof(*mp)); - autodetect_mpu_icm(mp); + struct mpu9250 *mp = oid_alloc(args[0], command_config_mpu9250 + , sizeof(*mp)); mp->timer.func = mpu9250_event; mp->i2c = i2cdev_oid_lookup(args[1]); } @@ -118,12 +82,7 @@ read_mpu(struct i2cdev_s *i2c, uint8_t reg_len, uint8_t *reg static uint16_t get_fifo_status(struct mpu9250 *mp) { - uint8_t reg[1]; - if (mp->variant == MPU) { - reg[0] = AR_FIFO_COUNT_H; - } else { - reg[0] = ICM_AR_FIFO_COUNT_H; - } + uint8_t reg[] = {AR_FIFO_COUNT_H}; uint8_t msg[2]; read_mpu(mp->i2c, sizeof(reg), reg, sizeof(msg), msg); uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1]; @@ -140,16 +99,9 @@ mp9250_query(struct mpu9250 *mp, uint8_t oid) if (mp->fifo_pkts_bytes < BYTES_PER_BLOCK) mp->fifo_pkts_bytes = get_fifo_status(mp); - autodetect_mpu_icm(mp); - // If we have enough bytes to fill the buffer do it and send report if (mp->fifo_pkts_bytes >= BYTES_PER_BLOCK) { - uint8_t reg; - if (mp->variant == MPU) { - reg = AR_FIFO; - } else { - reg = ICM_AR_FIFO; - } + uint8_t reg = AR_FIFO; read_mpu(mp->i2c, sizeof(reg), ®, BYTES_PER_BLOCK, &mp->sb.data[0]); mp->sb.data_count = BYTES_PER_BLOCK; mp->fifo_pkts_bytes -= BYTES_PER_BLOCK; @@ -171,8 +123,6 @@ command_query_mpu9250(uint32_t *args) { struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250); - autodetect_mpu_icm(mp); - sched_del_timer(&mp->timer); mp->flags = 0; if (!args[1]) { @@ -197,31 +147,15 @@ command_query_mpu9250_status(uint32_t *args) { struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250); - autodetect_mpu_icm(mp); - // Detect if a FIFO overrun occurred - uint8_t int_reg[1]; - if (mp->variant == MPU) { - int_reg[0] = AR_INT_STATUS; - } else { - int_reg[0] = ICM_AR_INT_STATUS; - } + uint8_t int_reg[] = {AR_INT_STATUS}; uint8_t int_msg; read_mpu(mp->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), &int_msg); - if (mp->variant == MPU) { - if (int_msg & FIFO_OVERFLOW_INT ) - mp->sb.possible_overflows++; - } else { - if (int_msg & ICM_FIFO_OVERFLOW_INT ) + if (int_msg & FIFO_OVERFLOW_INT) mp->sb.possible_overflows++; - } + // Read latest FIFO count (with precise timing) - uint8_t reg[1]; - if (mp->variant == MPU) { - reg[0] = AR_FIFO_COUNT_H; - } else { - reg[0] = ICM_AR_FIFO_COUNT_H; - } + uint8_t reg[] = {AR_FIFO_COUNT_H}; uint8_t msg[2]; uint32_t time1 = timer_read_time(); read_mpu(mp->i2c, sizeof(reg), reg, sizeof(msg), msg);