Skip to content

Commit

Permalink
sensor_mpu9250: No need to schedule start of bulk reading
Browse files Browse the repository at this point in the history 
It's simpler and faster to enable the mpu9250 in the python code.

Signed-off-by: Kevin O'Connor <[email protected]>
  • Loading branch information
@KevinOConnor
KevinOConnor committed Jan 19, 2024
1 parent 6f0e91f commit d853c19
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 108 deletions.
26 changes: 16 additions & 10 deletions klippy/extras/mpu9250.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
REG_USER_CTRL = 0x6A
REG_PWR_MGMT_1 = 0x6B
REG_PWR_MGMT_2 = 0x6C
REG_INT_STATUS = 0x3A

SAMPLE_RATE_DIVS = { 4000:0x00 }

Expand All @@ -40,15 +41,17 @@
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
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

Expand Down Expand Up @@ -90,10 +93,10 @@ def _build_config(self):
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):
Expand Down Expand Up @@ -164,22 +167,25 @@ def _start_measurements(self):
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)
Expand Down
125 changes: 27 additions & 98 deletions src/sensor_mpu9250.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
//
// Copyright (C) 2023 Matthew Swabey <[email protected]>
// Copyright (C) 2022 Harry Beyel <[email protected]>
// Copyright (C) 2020-2021 Kevin O'Connor <[email protected]>
// Copyright (C) 2020-2023 Kevin O'Connor <[email protected]>
//
// This file may be distributed under the terms of the GNU GPLv3 license.

Expand All @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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);

0 comments on commit d853c19

Please sign in to comment.