From f55b9d3e5746b73d37f1f2de288aa06d9fe23138 Mon Sep 17 00:00:00 2001 From: bluesforte Date: Wed, 4 May 2022 16:02:37 -0700 Subject: [PATCH] mpu9250: Adding support for MPU-9250 (and MPU-6050) accelerometer Add support for mpu9250 accelerometer over I2C bus. Signed-off-by: Harry Beyel --- docs/Measuring_Resonances.md | 28 ++- klippy/extras/mpu9250.py | 461 +++++++++++++++++++++++++++++++++++ src/Makefile | 1 + src/i2ccmds.c | 11 +- src/i2ccmds.h | 13 + src/sensor_mpu9250.c | 277 +++++++++++++++++++++ 6 files changed, 786 insertions(+), 5 deletions(-) create mode 100644 klippy/extras/mpu9250.py create mode 100644 src/i2ccmds.h create mode 100644 src/sensor_mpu9250.c diff --git a/docs/Measuring_Resonances.md b/docs/Measuring_Resonances.md index 655a6494..42bb9fb7 100644 --- a/docs/Measuring_Resonances.md +++ b/docs/Measuring_Resonances.md @@ -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 diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py new file mode 100644 index 00000000..a8f35bc6 --- /dev/null +++ b/klippy/extras/mpu9250.py @@ -0,0 +1,461 @@ +# Support for reading acceleration data from an mpu9250 chip +# +# 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. +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) diff --git a/src/Makefile b/src/Makefile index 98c91a30..f5c32f1a 100644 --- a/src/Makefile +++ b/src/Makefile @@ -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 diff --git a/src/i2ccmds.c b/src/i2ccmds.c index cde0f6fa..69af011b 100644 --- a/src/i2ccmds.c +++ b/src/i2ccmds.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) { diff --git a/src/i2ccmds.h b/src/i2ccmds.h new file mode 100644 index 00000000..49c05c93 --- /dev/null +++ b/src/i2ccmds.h @@ -0,0 +1,13 @@ +#ifndef __I2CCMDS_H +#define __I2CCMDS_H + +#include +#include "board/gpio.h" // i2c_config + +struct i2cdev_s { + struct i2c_config i2c_config; +}; + +struct i2cdev_s *i2cdev_oid_lookup(uint8_t oid); + +#endif diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c new file mode 100644 index 00000000..d7f30928 --- /dev/null +++ b/src/sensor_mpu9250.c @@ -0,0 +1,277 @@ +// Support for gathering acceleration data from mpu9250 chip +// +// 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. + +#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 "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);