From 117090435267130b4ec65be88637fc443784c6dd Mon Sep 17 00:00:00 2001 From: phansel Date: Mon, 2 Dec 2024 15:19:05 -0800 Subject: [PATCH] attempt at unification; cannot resolve i2c / fixed frequency bus device init --- klippy/extras/icm20948.py | 175 ------------------------------------ klippy/extras/mpu9250.py | 94 ++++++++++++++++++- src/Makefile | 2 +- src/sensor_icm20948.c | 184 -------------------------------------- src/sensor_mpu9250.c | 82 +++++++++++++++-- 5 files changed, 165 insertions(+), 372 deletions(-) delete mode 100644 klippy/extras/icm20948.py delete mode 100644 src/sensor_icm20948.c diff --git a/klippy/extras/icm20948.py b/klippy/extras/icm20948.py deleted file mode 100644 index 407e8501c38d..000000000000 --- a/klippy/extras/icm20948.py +++ /dev/null @@ -1,175 +0,0 @@ -# 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 ff15fed41be6..81e45564a500 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -16,10 +16,14 @@ 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 @@ -61,6 +65,7 @@ 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 @@ -69,8 +74,10 @@ 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") @@ -83,6 +90,23 @@ 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" @@ -93,6 +117,7 @@ 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] @@ -118,6 +143,9 @@ 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" @@ -126,6 +154,7 @@ 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) @@ -133,11 +162,20 @@ 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 - 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) + 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_ACCEL_CONFIG, SET_ACCEL_CONFIG) - self.set_reg(REG_ACCEL_CONFIG2, SET_ACCEL_CONFIG2) + if not self.this_mpu9250_is_icm20948: + 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) @@ -168,6 +206,54 @@ 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/Makefile b/src/Makefile index 7721ccdbab78..86c7407e687f 100644 --- a/src/Makefile +++ b/src/Makefile @@ -16,7 +16,7 @@ src-$(CONFIG_WANT_SOFTWARE_SPI) += spi_software.c src-$(CONFIG_WANT_SOFTWARE_I2C) += i2c_software.c sensors-src-$(CONFIG_HAVE_GPIO_SPI) := thermocouple.c sensor_adxl345.c \ sensor_angle.c -sensors-src-$(CONFIG_HAVE_GPIO_I2C) += sensor_mpu9250.c sensor_icm20948.c +sensors-src-$(CONFIG_HAVE_GPIO_I2C) += sensor_mpu9250.c src-$(CONFIG_WANT_SENSORS) += $(sensors-src-y) src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c src-$(CONFIG_WANT_LDC1612) += sensor_ldc1612.c diff --git a/src/sensor_icm20948.c b/src/sensor_icm20948.c deleted file mode 100644 index eb73a52c15b1..000000000000 --- a/src/sensor_icm20948.c +++ /dev/null @@ -1,184 +0,0 @@ -// 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 7476734c4f0a..fa8fa51ec863 100644 --- a/src/sensor_mpu9250.c +++ b/src/sensor_mpu9250.c @@ -22,9 +22,24 @@ #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; @@ -32,6 +47,7 @@ struct mpu9250 { uint16_t fifo_max, fifo_pkts_bytes; uint8_t flags; struct sensor_bulk sb; + enum mp_type variant; }; enum { @@ -40,11 +56,30 @@ 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; @@ -53,8 +88,9 @@ mpu9250_event(struct timer *timer) void command_config_mpu9250(uint32_t *args) { - struct mpu9250 *mp = oid_alloc(args[0], command_config_mpu9250 - , sizeof(*mp)); + struct mpu9250 *mp = oid_alloc(args[0], command_config_mpu9250, + sizeof(*mp)); + autodetect_mpu_icm(mp); mp->timer.func = mpu9250_event; mp->i2c = i2cdev_oid_lookup(args[1]); } @@ -82,7 +118,12 @@ 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[] = {AR_FIFO_COUNT_H}; + uint8_t reg[1]; + if (mp->variant == MPU) { + reg[0] = AR_FIFO_COUNT_H; + } else { + reg[0] = ICM_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]; @@ -99,9 +140,16 @@ 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 = AR_FIFO; + uint8_t reg; + if (mp->variant == MPU) { + reg = AR_FIFO; + } else { + reg = ICM_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; @@ -123,6 +171,8 @@ 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]) { @@ -147,15 +197,31 @@ 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[] = {AR_INT_STATUS}; + 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_msg; read_mpu(mp->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), &int_msg); - if (int_msg & FIFO_OVERFLOW_INT) + if (mp->variant == MPU) { + if (int_msg & FIFO_OVERFLOW_INT ) + mp->sb.possible_overflows++; + } else { + if (int_msg & ICM_FIFO_OVERFLOW_INT ) mp->sb.possible_overflows++; - + } // Read latest FIFO count (with precise timing) - uint8_t reg[] = {AR_FIFO_COUNT_H}; + uint8_t reg[1]; + if (mp->variant == MPU) { + reg[0] = AR_FIFO_COUNT_H; + } else { + reg[0] = ICM_AR_FIFO_COUNT_H; + } uint8_t msg[2]; uint32_t time1 = timer_read_time(); read_mpu(mp->i2c, sizeof(reg), reg, sizeof(msg), msg);