mirror of
https://github.com/Klipper3d/klipper.git
synced 2025-08-07 22:14:05 -06:00
mpu9250: Adding support for MPU-9250 (and MPU-6050) accelerometer
Add support for mpu9250 accelerometer over I2C bus. Signed-off-by: Harry Beyel <harry3b9@gmail.com>
This commit is contained in:
bluesforte
Kevin O'Connor
parent
fc7838855f
commit
f55b9d3e57
6 changed files with 786 additions and 5 deletions
|
|
@ -8,5 +8,6 @@ src-$(CONFIG_HAVE_GPIO_SPI) += spicmds.c thermocouple.c
|
|||
src-$(CONFIG_HAVE_GPIO_I2C) += i2ccmds.c
|
||||
src-$(CONFIG_HAVE_GPIO_HARD_PWM) += pwmcmds.c
|
||||
bb-src-$(CONFIG_HAVE_GPIO_SPI) := spi_software.c sensor_adxl345.c sensor_angle.c
|
||||
bb-src-$(CONFIG_HAVE_GPIO_I2C) += sensor_mpu9250.c
|
||||
src-$(CONFIG_HAVE_GPIO_BITBANGING) += $(bb-src-y) lcd_st7920.c lcd_hd44780.c \
|
||||
buttons.c tmcuart.c neopixel.c pulse_counter.c
|
||||
|
|
|
|||
|
|
@ -9,10 +9,7 @@
|
|||
#include "command.h" //sendf
|
||||
#include "sched.h" //DECL_COMMAND
|
||||
#include "board/gpio.h" //i2c_write/read/setup
|
||||
|
||||
struct i2cdev_s {
|
||||
struct i2c_config i2c_config;
|
||||
};
|
||||
#include "i2ccmds.h"
|
||||
|
||||
void
|
||||
command_config_i2c(uint32_t *args)
|
||||
|
|
@ -25,6 +22,12 @@ command_config_i2c(uint32_t *args)
|
|||
DECL_COMMAND(command_config_i2c,
|
||||
"config_i2c oid=%c i2c_bus=%u rate=%u address=%u");
|
||||
|
||||
struct i2cdev_s *
|
||||
i2cdev_oid_lookup(uint8_t oid)
|
||||
{
|
||||
return oid_lookup(oid, command_config_i2c);
|
||||
}
|
||||
|
||||
void
|
||||
command_i2c_write(uint32_t *args)
|
||||
{
|
||||
|
|
|
|||
13
src/i2ccmds.h
Normal file
13
src/i2ccmds.h
Normal file
|
|
@ -0,0 +1,13 @@
|
|||
#ifndef __I2CCMDS_H
|
||||
#define __I2CCMDS_H
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "board/gpio.h" // i2c_config
|
||||
|
||||
struct i2cdev_s {
|
||||
struct i2c_config i2c_config;
|
||||
};
|
||||
|
||||
struct i2cdev_s *i2cdev_oid_lookup(uint8_t oid);
|
||||
|
||||
#endif
|
||||
277
src/sensor_mpu9250.c
Normal file
277
src/sensor_mpu9250.c
Normal file
|
|
@ -0,0 +1,277 @@
|
|||
// Support for gathering acceleration data from mpu9250 chip
|
||||
//
|
||||
// Copyright (C) 2022 Harry Beyel <harry3b9@gmail.com>
|
||||
// Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
|
||||
//
|
||||
// This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
|
||||
#include <string.h> // 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 "board/gpio.h" // i2c_read
|
||||
#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 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 BYTES_PER_FIFO_ENTRY 6
|
||||
|
||||
struct mpu9250 {
|
||||
struct timer timer;
|
||||
uint32_t rest_ticks;
|
||||
struct i2cdev_s *i2c;
|
||||
uint16_t sequence, limit_count;
|
||||
uint8_t flags, data_count;
|
||||
// data size must be <= 255 due to i2c api
|
||||
// = SAMPLES_PER_BLOCK (from mpu9250.py) * BYTES_PER_FIFO_ENTRY + 1
|
||||
uint8_t data[48];
|
||||
};
|
||||
|
||||
enum {
|
||||
AX_HAVE_START = 1<<0, AX_RUNNING = 1<<1, AX_PENDING = 1<<2,
|
||||
};
|
||||
|
||||
static struct task_wake mpu9250_wake;
|
||||
|
||||
// Reads the fifo byte count from the device.
|
||||
uint16_t
|
||||
get_fifo_status (struct mpu9250 *mp)
|
||||
{
|
||||
uint8_t regs[] = {AR_FIFO_COUNT_H};
|
||||
uint8_t msg[2];
|
||||
i2c_read(mp->i2c->i2c_config, sizeof(regs), regs, 2, msg);
|
||||
msg[0] = 0x1F & msg[0]; // discard 3 MSB per datasheet
|
||||
return (((uint16_t)msg[0]) << 8 | msg[1]);
|
||||
}
|
||||
|
||||
// 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);
|
||||
ax->flags |= AX_PENDING;
|
||||
sched_wake_task(&mpu9250_wake);
|
||||
return SF_DONE;
|
||||
}
|
||||
|
||||
void
|
||||
command_config_mpu9250(uint32_t *args)
|
||||
{
|
||||
struct mpu9250 *mp = oid_alloc(args[0], command_config_mpu9250
|
||||
, sizeof(*mp));
|
||||
mp->timer.func = mpu9250_event;
|
||||
mp->i2c = i2cdev_oid_lookup(args[1]);
|
||||
}
|
||||
DECL_COMMAND(command_config_mpu9250, "config_mpu9250 oid=%c i2c_oid=%c");
|
||||
|
||||
// Report local measurement buffer
|
||||
static void
|
||||
mp9250_report(struct mpu9250 *mp, uint8_t oid)
|
||||
{
|
||||
sendf("mpu9250_data oid=%c sequence=%hu data=%*s"
|
||||
, oid, mp->sequence, mp->data_count, mp->data);
|
||||
mp->data_count = 0;
|
||||
mp->sequence++;
|
||||
}
|
||||
|
||||
// Report buffer and fifo status
|
||||
static void
|
||||
mp9250_status(struct mpu9250 *mp, uint_fast8_t oid
|
||||
, uint32_t time1, uint32_t time2, uint16_t fifo)
|
||||
{
|
||||
sendf("mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
|
||||
" buffered=%c fifo=%u limit_count=%hu"
|
||||
, oid, time1, time2-time1, mp->sequence
|
||||
, mp->data_count, fifo, mp->limit_count);
|
||||
}
|
||||
|
||||
// Helper code to reschedule the mpu9250_event() timer
|
||||
static void
|
||||
mp9250_reschedule_timer(struct mpu9250 *mp)
|
||||
{
|
||||
irq_disable();
|
||||
mp->timer.waketime = timer_read_time() + mp->rest_ticks;
|
||||
sched_add_timer(&mp->timer);
|
||||
irq_enable();
|
||||
}
|
||||
|
||||
// Query accelerometer data
|
||||
static void
|
||||
mp9250_query(struct mpu9250 *mp, uint8_t oid)
|
||||
{
|
||||
// Check fifo status
|
||||
uint16_t fifo_bytes = get_fifo_status(mp);
|
||||
if (fifo_bytes >= AR_FIFO_SIZE - BYTES_PER_FIFO_ENTRY)
|
||||
mp->limit_count++;
|
||||
|
||||
// Read data
|
||||
// FIFO data are: [Xh, Xl, Yh, Yl, Zh, Zl]
|
||||
uint8_t reg = AR_FIFO;
|
||||
uint8_t bytes_to_read = fifo_bytes < sizeof(mp->data) - mp->data_count ?
|
||||
fifo_bytes & 0xFF :
|
||||
(sizeof(mp->data) - mp->data_count) & 0xFF;
|
||||
|
||||
// round down to nearest full packet of data
|
||||
bytes_to_read = bytes_to_read / BYTES_PER_FIFO_ENTRY * BYTES_PER_FIFO_ENTRY;
|
||||
|
||||
// Extract x, y, z measurements into data holder and report
|
||||
if (bytes_to_read > 0) {
|
||||
i2c_read(mp->i2c->i2c_config, sizeof(reg), ®,
|
||||
bytes_to_read, &mp->data[mp->data_count]);
|
||||
mp->data_count += bytes_to_read;
|
||||
|
||||
// report data when buffer is full
|
||||
if (mp->data_count + BYTES_PER_FIFO_ENTRY > sizeof(mp->data)) {
|
||||
mp9250_report(mp, oid);
|
||||
}
|
||||
}
|
||||
|
||||
// check if we need to run the task again (more packets in fifo?)
|
||||
if ( bytes_to_read > 0 &&
|
||||
bytes_to_read / BYTES_PER_FIFO_ENTRY <
|
||||
fifo_bytes / BYTES_PER_FIFO_ENTRY) {
|
||||
// more data still ready in the fifo buffer
|
||||
sched_wake_task(&mpu9250_wake);
|
||||
}
|
||||
else if (mp->flags & AX_RUNNING) {
|
||||
// No more fifo data, but actively running. Sleep until next check
|
||||
sched_del_timer(&mp->timer);
|
||||
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);
|
||||
|
||||
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 };
|
||||
uint32_t end1_time = timer_read_time();
|
||||
i2c_write(mp->i2c->i2c_config, sizeof(msg), msg);
|
||||
uint32_t end2_time = timer_read_time();
|
||||
|
||||
// Drain any measurements still in fifo
|
||||
uint16_t fifo_bytes = get_fifo_status(mp);
|
||||
while (fifo_bytes >= BYTES_PER_FIFO_ENTRY) {
|
||||
mp9250_query(mp, oid);
|
||||
fifo_bytes = get_fifo_status(mp);
|
||||
}
|
||||
|
||||
// Report final data
|
||||
if (mp->data_count > 0)
|
||||
mp9250_report(mp, oid);
|
||||
mp9250_status(mp, oid, end1_time, end2_time,
|
||||
fifo_bytes / BYTES_PER_FIFO_ENTRY);
|
||||
}
|
||||
|
||||
void
|
||||
command_query_mpu9250(uint32_t *args)
|
||||
{
|
||||
struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250);
|
||||
|
||||
if (!args[2]) {
|
||||
// End measurements
|
||||
mp9250_stop(mp, args[0]);
|
||||
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->sequence = mp->limit_count = 0;
|
||||
mp->data_count = 0;
|
||||
sched_add_timer(&mp->timer);
|
||||
}
|
||||
DECL_COMMAND(command_query_mpu9250,
|
||||
"query_mpu9250 oid=%c clock=%u rest_ticks=%u");
|
||||
|
||||
void
|
||||
command_query_mpu9250_status(uint32_t *args)
|
||||
{
|
||||
struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250);
|
||||
uint8_t msg[2];
|
||||
uint32_t time1 = timer_read_time();
|
||||
uint8_t regs[] = {AR_FIFO_COUNT_H};
|
||||
i2c_read(mp->i2c->i2c_config, 1, regs, 2, msg);
|
||||
uint32_t time2 = timer_read_time();
|
||||
msg[0] = 0x1F & msg[0]; // discard 3 MSB
|
||||
uint16_t fifo_bytes = (((uint16_t)msg[0]) << 8) | msg[1];
|
||||
mp9250_status(mp, args[0], time1, time2, fifo_bytes / BYTES_PER_FIFO_ENTRY);
|
||||
}
|
||||
DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c");
|
||||
|
||||
void
|
||||
mpu9250_task(void)
|
||||
{
|
||||
if (!sched_check_wake(&mpu9250_wake))
|
||||
return;
|
||||
uint8_t oid;
|
||||
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 {
|
||||
mp9250_query(mp, oid);
|
||||
}
|
||||
}
|
||||
}
|
||||
DECL_TASK(mpu9250_task);
|
||||
Loading…
Add table
Add a link
Reference in a new issue