Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

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 2022-05-04 16:02:37 -07:00 committed by Kevin O'Connor
parent fc7838855f
commit f55b9d3e57
6 changed files with 786 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
docs/Measuring_Resonances.md
View File

@ -31,6 +31,17 @@ and **will not work**. The recommended connection scheme:
| SDA | 19 | GPIO10 (SPI0_MOSI) |
| SCL | 23 | GPIO11 (SPI0_SCLK) |
An alternative to the ADXL345 is the MPU-9250 (or MPU-6050). This accelerometer has been tested to work over I2C on the RPi at 400kbaud.
Recommended connection scheme for I2C:
| MPU-9250 pin | RPi pin | RPi pin name |
|:--:|:--:|:--:|
| 3V3 (or VCC) | 01 | 3.3v DC power |
| GND | 09 | Ground |
| SDA | 03 | GPIO02 (SDA1) |
| SCL | 05 | GPIO03 (SCL1) |
Fritzing wiring diagrams for some of the ADXL345 boards:
![ADXL345-Rpi](img/adxl345-fritzing.png)
@ -87,7 +98,7 @@ Afterwards, check and follow the instructions in the
Make sure the Linux SPI driver is enabled by running `sudo
raspi-config` and enabling SPI under the "Interfacing options" menu.
Add the following to the printer.cfg file:
For the ADXL345, add the following to the printer.cfg file:
```
[mcu rpi]
serial: /tmp/klipper_host_mcu
@ -103,6 +114,21 @@ probe_points:
It is advised to start with 1 probe point, in the middle of the print bed,
slightly above it.
For the MPU-9250:
```
[mcu rpi]
serial: /tmp/klipper_host_mcu
[mpu9250]
i2c_mcu: rpi
i2c_bus: i2c.1
[resonance_tester]
accel_chip: mpu9250
probe_points:
100, 100, 20 # an example
```
Restart Klipper via the `RESTART` command.
## Measuring the resonances

461
klippy/extras/mpu9250.py Normal file
View File

@ -0,0 +1,461 @@
# Support for reading acceleration data from an 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.
import logging, time, collections, threading, multiprocessing, os
from . import bus, motion_report
MPU9250_ADDR = 0x68
MPU9250_DEV_ID = 0x73
MPU6050_DEV_ID = 0x68
# MPU9250 registers
REG_DEVID = 0x75
REG_FIFO_EN = 0x23
REG_SMPLRT_DIV = 0x19
REG_CONFIG = 0x1A
REG_ACCEL_CONFIG = 0x1C
REG_ACCEL_CONFIG2 = 0x1D
REG_USER_CTRL = 0x6A
REG_PWR_MGMT_1 = 0x6B
REG_PWR_MGMT_2 = 0x6C
SAMPLE_RATE_DIVS = { 4000:0x00 }
SET_CONFIG = 0x01 # FIFO mode 'stream' style
SET_ACCEL_CONFIG = 0x10 # 8g full scale
SET_ACCEL_CONFIG2 = 0x08 # 1046Hz BW, 0.503ms delay 4kHz sample rate
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
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
Accel_Measurement = collections.namedtuple(
'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z'))
# Helper method for getting the two's complement value of an unsigned int
def twos_complement(val, nbits):
if (val & (1 << (nbits - 1))) != 0:
val = val - (1 << nbits)
return val
# Helper class to obtain measurements
class MPU9250QueryHelper:
def __init__(self, printer, cconn):
self.printer = printer
self.cconn = cconn
print_time = printer.lookup_object('toolhead').get_last_move_time()
self.request_start_time = self.request_end_time = print_time
self.samples = self.raw_samples = []
def finish_measurements(self):
toolhead = self.printer.lookup_object('toolhead')
self.request_end_time = toolhead.get_last_move_time()
toolhead.wait_moves()
self.cconn.finalize()
def _get_raw_samples(self):
raw_samples = self.cconn.get_messages()
if raw_samples:
self.raw_samples = raw_samples
return self.raw_samples
def has_valid_samples(self):
raw_samples = self._get_raw_samples()
for msg in raw_samples:
data = msg['params']['data']
first_sample_time = data[0][0]
last_sample_time = data[-1][0]
if (first_sample_time > self.request_end_time
or last_sample_time < self.request_start_time):
continue
# The time intervals [first_sample_time, last_sample_time]
# and [request_start_time, request_end_time] have non-zero
# intersection. It is still theoretically possible that none
# of the samples from raw_samples fall into the time interval
# [request_start_time, request_end_time] if it is too narrow
# or on very heavy data losses. In practice, that interval
# is at least 1 second, so this possibility is negligible.
return True
return False
def get_samples(self):
raw_samples = self._get_raw_samples()
if not raw_samples:
return self.samples
total = sum([len(m['params']['data']) for m in raw_samples])
count = 0
self.samples = samples = [None] * total
for msg in raw_samples:
for samp_time, x, y, z in msg['params']['data']:
if samp_time < self.request_start_time:
continue
if samp_time > self.request_end_time:
break
samples[count] = Accel_Measurement(samp_time, x, y, z)
count += 1
del samples[count:]
return self.samples
def write_to_file(self, filename):
def write_impl():
try:
# Try to re-nice writing process
os.nice(20)
except:
pass
f = open(filename, "w")
f.write("#time,accel_x,accel_y,accel_z\n")
samples = self.samples or self.get_samples()
for t, accel_x, accel_y, accel_z in samples:
f.write("%.6f,%.6f,%.6f,%.6f\n" % (
t, accel_x, accel_y, accel_z))
f.close()
write_proc = multiprocessing.Process(target=write_impl)
write_proc.daemon = True
write_proc.start()
# Helper class for G-Code commands
class MPU9250CommandHelper:
def __init__(self, config, chip):
self.printer = config.get_printer()
self.chip = chip
self.bg_client = None
self.name = config.get_name().split()[-1]
self.register_commands(self.name)
if self.name == "mpu9250":
self.register_commands(None)
def register_commands(self, name):
# Register commands
gcode = self.printer.lookup_object('gcode')
gcode.register_mux_command("ACCELEROMETER_MEASURE", "CHIP", name,
self.cmd_ACCELEROMETER_MEASURE,
desc=self.cmd_ACCELEROMETER_MEASURE_help)
gcode.register_mux_command("ACCELEROMETER_QUERY", "CHIP", name,
self.cmd_ACCELEROMETER_QUERY,
desc=self.cmd_ACCELEROMETER_QUERY_help)
gcode.register_mux_command("ACCELEROMETER_DEBUG_READ", "CHIP", name,
self.cmd_ACCELEROMETER_DEBUG_READ,
desc=self.cmd_ACCELEROMETER_DEBUG_READ_help)
gcode.register_mux_command("ACCELEROMETER_DEBUG_WRITE", "CHIP", name,
self.cmd_ACCELEROMETER_DEBUG_WRITE,
desc=self.cmd_ACCELEROMETER_DEBUG_WRITE_help)
cmd_ACCELEROMETER_MEASURE_help = "Start/stop accelerometer"
def cmd_ACCELEROMETER_MEASURE(self, gcmd):
if self.bg_client is None:
# Start measurements
self.bg_client = self.chip.start_internal_client()
gcmd.respond_info("mpu9250 measurements started")
return
# End measurements
name = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
if not name.replace('-', '').replace('_', '').isalnum():
raise gcmd.error("Invalid mpu9250 NAME parameter")
bg_client = self.bg_client
self.bg_client = None
bg_client.finish_measurements()
# Write data to file
if self.name == "mpu9250":
filename = "/tmp/mpu9250-%s.csv" % (name,)
else:
filename = "/tmp/mpu9250-%s-%s.csv" % (self.name, name,)
bg_client.write_to_file(filename)
gcmd.respond_info("Writing raw accelerometer data to %s file"
% (filename,))
cmd_ACCELEROMETER_QUERY_help = "Query accelerometer for the current values"
def cmd_ACCELEROMETER_QUERY(self, gcmd):
aclient = self.chip.start_internal_client()
self.printer.lookup_object('toolhead').dwell(1.)
aclient.finish_measurements()
values = aclient.get_samples()
if not values:
raise gcmd.error("No mpu9250 measurements found")
_, accel_x, accel_y, accel_z = values[-1]
gcmd.respond_info("mpu9250 values (x, y, z): %.6f, %.6f, %.6f"
% (accel_x, accel_y, accel_z))
cmd_ACCELEROMETER_DEBUG_READ_help = "Query mpu9250 register (for debugging)"
def cmd_ACCELEROMETER_DEBUG_READ(self, gcmd):
reg = gcmd.get("REG", minval=0, maxval=126, parser=lambda x: int(x, 0))
val = self.chip.read_reg(reg)
gcmd.respond_info("MPU9250 REG[0x%x] = 0x%x" % (reg, val))
cmd_ACCELEROMETER_DEBUG_WRITE_help = "Set mpu9250 register (for debugging)"
def cmd_ACCELEROMETER_DEBUG_WRITE(self, gcmd):
reg = gcmd.get("REG", minval=0, maxval=126, parser=lambda x: int(x, 0))
val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0))
self.chip.set_reg(reg, val)
# Helper class for chip clock synchronization via linear regression
class ClockSyncRegression:
def __init__(self, mcu, chip_clock_smooth, decay = 1. / 20.):
self.mcu = mcu
self.chip_clock_smooth = chip_clock_smooth
self.decay = decay
self.last_chip_clock = self.last_exp_mcu_clock = 0.
self.mcu_clock_avg = self.mcu_clock_variance = 0.
self.chip_clock_avg = self.chip_clock_covariance = 0.
def reset(self, mcu_clock, chip_clock):
self.mcu_clock_avg = self.last_mcu_clock = mcu_clock
self.chip_clock_avg = chip_clock
self.mcu_clock_variance = self.chip_clock_covariance = 0.
self.last_chip_clock = self.last_exp_mcu_clock = 0.
def update(self, mcu_clock, chip_clock):
# Update linear regression
decay = self.decay
diff_mcu_clock = mcu_clock - self.mcu_clock_avg
self.mcu_clock_avg += decay * diff_mcu_clock
self.mcu_clock_variance = (1. - decay) * (
self.mcu_clock_variance + diff_mcu_clock**2 * decay)
diff_chip_clock = chip_clock - self.chip_clock_avg
self.chip_clock_avg += decay * diff_chip_clock
self.chip_clock_covariance = (1. - decay) * (
self.chip_clock_covariance + diff_mcu_clock*diff_chip_clock*decay)
def set_last_chip_clock(self, chip_clock):
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
self.last_chip_clock = chip_clock
self.last_exp_mcu_clock = base_mcu + (chip_clock-base_chip) * inv_cfreq
def get_clock_translation(self):
inv_chip_freq = self.mcu_clock_variance / self.chip_clock_covariance
if not self.last_chip_clock:
return self.mcu_clock_avg, self.chip_clock_avg, inv_chip_freq
# Find mcu clock associated with future chip_clock
s_chip_clock = self.last_chip_clock + self.chip_clock_smooth
scdiff = s_chip_clock - self.chip_clock_avg
s_mcu_clock = self.mcu_clock_avg + scdiff * inv_chip_freq
# Calculate frequency to converge at future point
mdiff = s_mcu_clock - self.last_exp_mcu_clock
s_inv_chip_freq = mdiff / self.chip_clock_smooth
return self.last_exp_mcu_clock, self.last_chip_clock, s_inv_chip_freq
def get_time_translation(self):
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
clock_to_print_time = self.mcu.clock_to_print_time
base_time = clock_to_print_time(base_mcu)
inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time
return base_time, base_chip, inv_freq
MIN_MSG_TIME = 0.100
BYTES_PER_SAMPLE = 6
SAMPLES_PER_BLOCK = 8
# Printer class that controls MPU9250 chip
class MPU9250:
def __init__(self, config):
self.printer = config.get_printer()
MPU9250CommandHelper(config, self)
self.query_rate = 0
am = {'x': (0, SCALE), 'y': (1, SCALE), 'z': (2, SCALE),
'-x': (0, -SCALE), '-y': (1, -SCALE), '-z': (2, -SCALE)}
axes_map = config.getlist('axes_map', ('x','y','z'), count=3)
if any([a not in am for a in axes_map]):
raise config.error("Invalid mpu9250 axes_map parameter")
self.axes_map = [am[a.strip()] for a in axes_map]
self.data_rate = config.getint('rate', 4000)
if self.data_rate not in SAMPLE_RATE_DIVS:
raise config.error("Invalid rate parameter: %d" % (self.data_rate,))
# Measurement storage (accessed from background thread)
self.lock = threading.Lock()
self.raw_samples = []
# Setup mcu sensor_mpu9250 bulk query code
self.i2c = bus.MCU_I2C_from_config(config,
default_addr=MPU9250_ADDR,
default_speed=400000)
self.mcu = mcu = self.i2c.get_mcu()
self.oid = oid = mcu.create_oid()
self.query_mpu9250_cmd = self.query_mpu9250_end_cmd = None
self.query_mpu9250_status_cmd = None
mcu.register_config_callback(self._build_config)
mcu.register_response(self._handle_mpu9250_data, "mpu9250_data", oid)
# Clock tracking
self.last_sequence = self.max_query_duration = 0
self.last_limit_count = self.last_error_count = 0
self.clock_sync = ClockSyncRegression(self.mcu, 640)
# API server endpoints
self.api_dump = motion_report.APIDumpHelper(
self.printer, self._api_update, self._api_startstop, 0.100)
self.name = config.get_name().split()[-1]
wh = self.printer.lookup_object('webhooks')
wh.register_mux_endpoint("mpu9250/dump_mpu9250", "sensor", self.name,
self._handle_dump_mpu9250)
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.oid,), on_restart=True)
self.query_mpu9250_cmd = self.mcu.lookup_command(
"query_mpu9250 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
self.query_mpu9250_end_cmd = self.mcu.lookup_query_command(
"query_mpu9250 oid=%c clock=%u rest_ticks=%u",
"mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
" buffered=%c fifo=%u limit_count=%hu", oid=self.oid, cq=cmdqueue)
self.query_mpu9250_status_cmd = self.mcu.lookup_query_command(
"query_mpu9250_status oid=%c",
"mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
" buffered=%c fifo=%u limit_count=%hu", 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)
# Measurement collection
def is_measuring(self):
return self.query_rate > 0
def _handle_mpu9250_data(self, params):
with self.lock:
self.raw_samples.append(params)
def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
last_sequence = self.last_sequence
time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
# Process every message in raw_samples
count = seq = 0
samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
for params in raw_samples:
seq_diff = (last_sequence - params['sequence']) & 0xffff
seq_diff -= (seq_diff & 0x8000) << 1
seq = last_sequence - seq_diff
d = bytearray(params['data'])
msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
for i in range(len(d) // BYTES_PER_SAMPLE):
d_xyz = d[i*BYTES_PER_SAMPLE:(i+1)*BYTES_PER_SAMPLE]
xhigh, xlow, yhigh, ylow, zhigh, zlow = d_xyz
rx = twos_complement(xhigh << 8 | xlow, 16)
ry = twos_complement(yhigh << 8 | ylow, 16)
rz = twos_complement(zhigh << 8 | zlow, 16)
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)
ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
samples[count] = (ptime, x, y, z)
count += 1
self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
del samples[count:]
return samples
def _update_clock(self, minclock=0):
# Query current state
for retry in range(5):
params = self.query_mpu9250_status_cmd.send([self.oid],
minclock=minclock)
fifo = params['fifo'] & 0x1fff
if fifo <= FIFO_SIZE:
break
else:
raise self.printer.command_error("Unable to query mpu9250 fifo")
mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
sequence = (self.last_sequence & ~0xffff) | params['next_sequence']
if sequence < self.last_sequence:
sequence += 0x10000
self.last_sequence = sequence
buffered = params['buffered']
limit_count = (self.last_limit_count & ~0xffff) | params['limit_count']
if limit_count < self.last_limit_count:
limit_count += 0x10000
self.last_limit_count = limit_count
duration = params['query_ticks']
if duration > self.max_query_duration:
# Skip measurement as a high query time could skew clock tracking
self.max_query_duration = max(2 * self.max_query_duration,
self.mcu.seconds_to_clock(.000005))
return
self.max_query_duration = 2 * duration
msg_count = (sequence * SAMPLES_PER_BLOCK
+ buffered // BYTES_PER_SAMPLE + fifo)
# The "chip clock" is the message counter plus .5 for average
# inaccuracy of query responses and plus .5 for assumed offset
# of mpu9250 hw processing time.
chip_clock = msg_count + 1
self.clock_sync.update(mcu_clock + duration // 2, chip_clock)
def _start_measurements(self):
if self.is_measuring():
return
# 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)
if dev_id != MPU9250_DEV_ID and dev_id != MPU6050_DEV_ID:
raise self.printer.command_error(
"Invalid mpu9250/mpu6050 id (got %x).\n"
"This is generally indicative of connection problems\n"
"(e.g. faulty wiring) or a faulty chip."
% (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)
time.sleep(20. / 1000) # wait for accelerometer chip wake up
self.set_reg(REG_SMPLRT_DIV, SAMPLE_RATE_DIVS[self.data_rate])
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)
# Setup samples
with self.lock:
self.raw_samples = []
# Start bulk reading
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(1. / self.data_rate)
self.query_rate = self.data_rate
self.query_mpu9250_cmd.send([self.oid, reqclock, rest_ticks],
reqclock=reqclock)
logging.info("MPU9250 starting '%s' measurements", self.name)
# Initialize clock tracking
self.last_sequence = 0
self.last_limit_count = self.last_error_count = 0
self.clock_sync.reset(reqclock, 0)
self.max_query_duration = 1 << 31
self._update_clock(minclock=reqclock)
self.max_query_duration = 1 << 31
def _finish_measurements(self):
if not self.is_measuring():
return
# Halt bulk reading
params = self.query_mpu9250_end_cmd.send([self.oid, 0, 0])
self.query_rate = 0
with self.lock:
self.raw_samples = []
logging.info("MPU9250 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)
# API interface
def _api_update(self, eventtime):
self._update_clock()
with self.lock:
raw_samples = self.raw_samples
self.raw_samples = []
if not raw_samples:
return {}
samples = self._extract_samples(raw_samples)
if not samples:
return {}
return {'data': samples, 'errors': self.last_error_count,
'overflows': self.last_limit_count}
def _api_startstop(self, is_start):
if is_start:
self._start_measurements()
else:
self._finish_measurements()
def _handle_dump_mpu9250(self, web_request):
self.api_dump.add_client(web_request)
hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration')
web_request.send({'header': hdr})
def start_internal_client(self):
cconn = self.api_dump.add_internal_client()
return MPU9250QueryHelper(self.printer, cconn)
def load_config(config):
return MPU9250(config)
def load_config_prefix(config):
return MPU9250(config)

1
src/Makefile
View File

@ -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

11
src/i2ccmds.c
View File

@ -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
View 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
View 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), &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);