From d853c1981107e6602c65285f91c805e8f33c3846 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 27 Dec 2023 12:02:10 -0500 Subject: [PATCH] sensor_mpu9250: No need to schedule start of bulk reading It's simpler and faster to enable the mpu9250 in the python code. Signed-off-by: Kevin O'Connor --- klippy/extras/mpu9250.py | 26 ++++---- src/sensor_mpu9250.c | 125 +++++++++------------------------------ 2 files changed, 43 insertions(+), 108 deletions(-) diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py index 421274ba..b4b8c439 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -30,6 +30,7 @@ REG_ACCEL_CONFIG2 = 0x1D REG_USER_CTRL = 0x6A REG_PWR_MGMT_1 = 0x6B REG_PWR_MGMT_2 = 0x6C +REG_INT_STATUS = 0x3A SAMPLE_RATE_DIVS = { 4000:0x00 } @@ -40,6 +41,10 @@ SET_PWR_MGMT_1_WAKE = 0x00 SET_PWR_MGMT_1_SLEEP= 0x40 SET_PWR_MGMT_2_ACCEL_ON = 0x07 SET_PWR_MGMT_2_OFF = 0x3F +SET_USER_FIFO_RESET = 0x04 +SET_USER_FIFO_EN = 0x40 +SET_ENABLE_FIFO = 0x08 +SET_DISABLE_FIFO = 0x00 FREEFALL_ACCEL = 9.80665 * 1000. # SCALE = 1/4096 g/LSB @8g scale * Earth gravity in mm/s**2 @@ -47,8 +52,6 @@ SCALE = 0.000244140625 * FREEFALL_ACCEL FIFO_SIZE = 512 -MIN_MSG_TIME = 0.100 - BYTES_PER_SAMPLE = 6 SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE @@ -90,10 +93,10 @@ class MPU9250: cmdqueue = self.i2c.get_command_queue() self.mcu.add_config_cmd("config_mpu9250 oid=%d i2c_oid=%d" % (self.oid, self.i2c.get_oid())) - self.mcu.add_config_cmd("query_mpu9250 oid=%d clock=0 rest_ticks=0" + self.mcu.add_config_cmd("query_mpu9250 oid=%d rest_ticks=0" % (self.oid,), on_restart=True) self.query_mpu9250_cmd = self.mcu.lookup_command( - "query_mpu9250 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue) + "query_mpu9250 oid=%c rest_ticks=%u", cq=cmdqueue) self.clock_updater.setup_query_command( self.mcu, "query_mpu9250_status oid=%c", oid=self.oid, cq=cmdqueue) def read_reg(self, reg): @@ -164,22 +167,25 @@ class MPU9250: 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) + # 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 self.bulk_queue.clear_samples() - systime = self.printer.get_reactor().monotonic() - print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME - reqclock = self.mcu.print_time_to_clock(print_time) rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate) - self.query_mpu9250_cmd.send([self.oid, reqclock, rest_ticks], - reqclock=reqclock) + self.query_mpu9250_cmd.send([self.oid, rest_ticks]) + self.set_reg(REG_FIFO_EN, SET_ENABLE_FIFO) logging.info("MPU9250 starting '%s' measurements", self.name) # Initialize clock tracking self.clock_updater.note_start() self.last_error_count = 0 def _finish_measurements(self): # Halt bulk reading - self.query_mpu9250_cmd.send_wait_ack([self.oid, 0, 0]) + self.set_reg(REG_FIFO_EN, SET_DISABLE_FIFO) + self.query_mpu9250_cmd.send_wait_ack([self.oid, 0]) self.bulk_queue.clear_samples() logging.info("MPU9250 finished '%s' measurements", self.name) self.set_reg(REG_PWR_MGMT_1, SET_PWR_MGMT_1_SLEEP) diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c index d52de811..23c02921 100644 --- a/src/sensor_mpu9250.c +++ b/src/sensor_mpu9250.c @@ -2,7 +2,7 @@ // // Copyright (C) 2023 Matthew Swabey // Copyright (C) 2022 Harry Beyel -// Copyright (C) 2020-2021 Kevin O'Connor +// Copyright (C) 2020-2023 Kevin O'Connor // // This file may be distributed under the terms of the GNU GPLv3 license. @@ -17,27 +17,10 @@ #include "i2ccmds.h" // i2cdev_oid_lookup // Chip registers -#define AR_FIFO_SIZE 512 - -#define AR_PWR_MGMT_1 0x6B -#define AR_PWR_MGMT_2 0x6C -#define AR_FIFO_EN 0x23 -#define AR_ACCEL_OUT_XH 0x3B -#define AR_USER_CTRL 0x6A #define AR_FIFO_COUNT_H 0x72 #define AR_FIFO 0x74 #define AR_INT_STATUS 0x3A -#define SET_ENABLE_FIFO 0x08 -#define SET_DISABLE_FIFO 0x00 -#define SET_USER_FIFO_RESET 0x04 -#define SET_USER_FIFO_EN 0x40 - -#define SET_PWR_SLEEP 0x40 -#define SET_PWR_WAKE 0x00 -#define SET_PWR_2_ACCEL 0x07 // only enable accelerometers -#define SET_PWR_2_NONE 0x3F // disable all sensors - #define FIFO_OVERFLOW_INT 0x10 #define BYTES_PER_FIFO_ENTRY 6 @@ -53,24 +36,11 @@ struct mpu9250 { }; enum { - AX_HAVE_START = 1<<0, AX_RUNNING = 1<<1, AX_PENDING = 1<<2, + AX_PENDING = 1<<0, }; static struct task_wake mpu9250_wake; -// Reads the fifo byte count from the device. -static uint16_t -get_fifo_status (struct mpu9250 *mp) -{ - uint8_t reg[] = {AR_FIFO_COUNT_H}; - uint8_t msg[2]; - i2c_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg); - uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1]; - if (fifo_bytes > mp->fifo_max) - mp->fifo_max = fifo_bytes; - return fifo_bytes; -} - // Event handler that wakes mpu9250_task() periodically static uint_fast8_t mpu9250_event(struct timer *timer) @@ -101,6 +71,19 @@ mp9250_reschedule_timer(struct mpu9250 *mp) irq_enable(); } +// Reads the fifo byte count from the device. +static uint16_t +get_fifo_status(struct mpu9250 *mp) +{ + uint8_t reg[] = {AR_FIFO_COUNT_H}; + uint8_t msg[2]; + i2c_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg); + uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1]; + if (fifo_bytes > mp->fifo_max) + mp->fifo_max = fifo_bytes; + return fifo_bytes; +} + // Query accelerometer data static void mp9250_query(struct mpu9250 *mp, uint8_t oid) @@ -123,82 +106,35 @@ mp9250_query(struct mpu9250 *mp, uint8_t oid) // otherwise schedule timed wakeup if (mp->fifo_pkts_bytes >= BYTES_PER_BLOCK) { sched_wake_task(&mpu9250_wake); - } else if (mp->flags & AX_RUNNING) { - sched_del_timer(&mp->timer); + } else { mp->flags &= ~AX_PENDING; mp9250_reschedule_timer(mp); } } -// Startup measurements -static void -mp9250_start(struct mpu9250 *mp, uint8_t oid) -{ - sched_del_timer(&mp->timer); - mp->flags = AX_RUNNING; - uint8_t msg[2]; - - msg[0] = AR_FIFO_EN; - msg[1] = SET_DISABLE_FIFO; // disable FIFO - i2c_write(mp->i2c->i2c_config, sizeof(msg), msg); - - msg[0] = AR_USER_CTRL; - msg[1] = SET_USER_FIFO_RESET; // reset FIFO buffer - i2c_write(mp->i2c->i2c_config, sizeof(msg), msg); - - msg[0] = AR_USER_CTRL; - msg[1] = SET_USER_FIFO_EN; // enable FIFO buffer access - i2c_write(mp->i2c->i2c_config, sizeof(msg), msg); - - uint8_t int_reg[] = {AR_INT_STATUS}; // clear FIFO overflow flag - i2c_read(mp->i2c->i2c_config, sizeof(int_reg), int_reg, 1, msg); - - msg[0] = AR_FIFO_EN; - msg[1] = SET_ENABLE_FIFO; // enable accel output to FIFO - i2c_write(mp->i2c->i2c_config, sizeof(msg), msg); - - mp9250_reschedule_timer(mp); -} - -// End measurements -static void -mp9250_stop(struct mpu9250 *mp, uint8_t oid) -{ - // Disable measurements - sched_del_timer(&mp->timer); - mp->flags = 0; - - // disable accel FIFO - uint8_t msg[2] = { AR_FIFO_EN, SET_DISABLE_FIFO }; - i2c_write(mp->i2c->i2c_config, sizeof(msg), msg); - - // Uncomment and rebuilt to check for FIFO overruns when tuning - //output("mpu9240 limit_count=%u fifo_max=%u", - // mp->limit_count, mp->fifo_max); -} - void command_query_mpu9250(uint32_t *args) { struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250); - if (!args[2]) { + sched_del_timer(&mp->timer); + mp->flags = 0; + if (!args[1]) { // End measurements - mp9250_stop(mp, args[0]); + + // Uncomment and rebuilt to check for FIFO overruns when tuning + //output("mpu9240 fifo_max=%u", mp->fifo_max); return; } + // Start new measurements query - sched_del_timer(&mp->timer); - mp->timer.waketime = args[1]; - mp->rest_ticks = args[2]; - mp->flags = AX_HAVE_START; + mp->rest_ticks = args[1]; sensor_bulk_reset(&mp->sb); mp->fifo_max = 0; mp->fifo_pkts_bytes = 0; - sched_add_timer(&mp->timer); + mp9250_reschedule_timer(mp); } -DECL_COMMAND(command_query_mpu9250, - "query_mpu9250 oid=%c clock=%u rest_ticks=%u"); +DECL_COMMAND(command_query_mpu9250, "query_mpu9250 oid=%c rest_ticks=%u"); void command_query_mpu9250_status(uint32_t *args) @@ -235,15 +171,8 @@ mpu9250_task(void) struct mpu9250 *mp; foreach_oid(oid, mp, command_config_mpu9250) { uint_fast8_t flags = mp->flags; - if (!(flags & AX_PENDING)) { - continue; - } - if (flags & AX_HAVE_START) { - mp9250_start(mp, oid); - } - else { + if (flags & AX_PENDING) mp9250_query(mp, oid); - } } } DECL_TASK(mpu9250_task);