// Copyright (c) 2026 Rigless Labs.
// SPDX-License-Identifier: MIT OR Apache-2.0
//! The MPU-9250, an inertial unit: a gyroscope and an accelerometer on a serial interface, a FIFO a
//! driver drains in bursts, and an I²C master inside the package with the [`Ak8963`] magnetometer die on
//! the far side of it.
//!
//! Written against the MPU-9250 Register Map and Descriptions, `RM-MPU-9250A-00` rev 1.4,
//! 9/9/2013, cited as **RM**, and the MPU-9250 Product Specification, `PS-MPU-9250A-01` rev 1.1,
//! 06/20/2016, cited as **PS**. Every section below is one or the other's.
//!
//! **The FIFO is derived from virtual time.** The part samples eight thousand times a second and a driver
//! reads a thousand times a second; nothing is scheduled per sample. The count of samples since the last
//! reset is computed at each select from the rate the registers give, and the samples not yet
//! materialised are generated then from the signals as held, into the FIFO and the data registers. What a
//! driver can observe is the same; what it cannot is a signal changing between two reads and the samples
//! either side of the change differing.
//!
//! | Register or section | Behaviour | Source | Test |
//! |---|---|---|---|
//! | The serial protocol | The first byte after the select falls is an address whose top bit is the direction, "Read (1) or Write (0)"; every byte after it is the next register, "Single or Burst Read/Writes" | PS 7.5 | `every_register_reads_and_writes_at_its_own_address_and_bursts_increment` |
//! | Reset values | `0x00` for every register "other than … Register 107 (0x01) Power Management 1 and Register 117 (0x71) `WHO_AM_I`" | RM 3 | `who_am_i_is_0x71`, `a_reset_restores_the_defaults_and_clears_itself` |
//! | `WHO_AM_I` | `0x71`. RM 4.38's header says "Reset value: 0x68" and its text "The default value of the register is 0x71"; the register table and the driver that reads this part say `0x71` | RM 3, 4.38 | `who_am_i_is_0x71` |
//! | `PWR_MGMT_1` | `H_RESET` "restores the default settings … the bit will auto clear"; `SLEEP` and `CLKSEL` 7 stop the sampling clock, the other codes run it | RM 4.34 | `a_reset_restores_the_defaults_and_clears_itself`, `a_sleeping_unit_takes_no_samples` |
//! | `PWR_MGMT_2` | An axis disabled reads zero: "Reset value: 0x00 (if sensor disabled)" | RM 4.35, 4.22 | `a_disabled_axis_reads_zero` |
//! | `USER_CTRL` | `FIFO_EN` enables the FIFO; `I2C_MST_EN` the master; `I2C_IF_DIS`, `FIFO_RST`, `I2C_MST_RST` and `SIG_COND_RST` "auto clear", the second emptying the FIFO, the third recomputing the external data allocation, the fourth clearing "all the sensor registers" | RM 4.33 | `a_fifo_reset_empties_it`, `the_allocation_is_recomputed_only_when_all_are_disabled_or_the_master_is_reset` |
//! | The sample rate | 8 kHz with `FCHOICE_B` non-zero or `DLPF_CFG` 0 or 7, else 1 kHz over `1 + SMPLRT_DIV`, the divider "only used for 1kHz internal sampling" | RM 4.4, 4.5, 4.6 | `the_sample_rate_follows_the_filter_and_the_divider` |
//! | The words | Gyro at `2^16 LSB / 500dps` halved per step of `GYRO_FS_SEL` — the 131, 65.5, 32.8 and 16.4 LSB/(°/s) of PS table 1 — less `X_OFFS_USR × 4 / 2^FS_SEL`; accelerometer at 16384, 8192, 4096 or 2048 LSB/g by `ACCEL_FS_SEL`, less the fifteen-bit offset in 0.98 mg steps; temperature `(TEMP_OUT − 0) / 333.87 + 21 °C`; big-endian, sixteen bits | RM 4.3, 4.22–4.24, 4.39; PS 3.1, 3.2, 3.4.2 | `the_words_are_the_signals_at_the_selected_range`, `the_temperature_word_is_the_specifications`, `the_offset_registers_survive_a_round_trip_and_are_subtracted` |
//! | `FIFO_EN` | Temperature, each gyro axis and the accelerometer written "at the sample rate", "in order of register number" | RM 4.11, 4.37 | `the_fifo_holds_what_fifo_en_says_in_register_order` |
//! | `FIFO_COUNT`, `FIFO_R_W` | 512 bytes; a thirteen-bit count, "Reading this byte latches the data for both"; a read takes the oldest byte, and empty "will return the last byte that was previously read"; full, `FIFO_OFLOW_INT` is set and the oldest is dropped, unless `FIFO_MODE` keeps the oldest and drops the new | RM 4.36, 4.37, 4.5; PS 4.17 | `the_fifo_count_is_the_samples_since_the_reset`, `an_empty_fifo_answers_the_last_byte`, `the_fifo_stops_at_512_under_fifo_mode_and_drops_the_oldest_otherwise` |
//! | `INT_STATUS`, `INT_PIN_CFG` | `RAW_DATA_RDY_INT` at each sample, `FIFO_OVERFLOW_INT`; "R/C", cleared by its own read, or by any read under `INT_ANYRD_2CLEAR` | RM 4.19–4.21 | `raw_data_ready_is_set_by_a_sample_and_cleared_as_int_pin_cfg_says` |
//! | The I²C master | Each enabled slave 0 to 3 with a length is run "at the Sample Rate … in order of Slave number", a read landing "in order of slave number, starting from `EXT_SENS_DATA_00`", a write sending its `DO` byte; `REG_DIS` skips the register byte; `I2C_MST_P_NSR` puts "a stop between reads" or a restart; a slave with `DLY_EN` runs "every (1+`I2C_MST_DLY`) samples"; the allocation is "recomputed only when (1) all slaves are disabled, or (2) the `I2C_MST_RST` bit is set"; slave 4 transfers once and its `EN` "is cleared when a single transfer is complete", a read landing in `I2C_SLV4_DI`; a slave that does not acknowledge sets its `NACK` bit in `I2C_MST_STATUS`, which a read clears | RM 4.12–4.18, 4.25–4.30 | `a_slave_read_lands_in_ext_sens_data_in_slave_order`, `a_slave_write_sends_its_do_byte`, `slave_4_transfers_once_and_clears_its_enable`, `a_slave_that_does_not_acknowledge_sets_its_nack_bit`, `the_allocation_is_recomputed_only_when_all_are_disabled_or_the_master_is_reset`, `a_delayed_slave_runs_every_n_plus_one_samples`, `the_master_puts_a_stop_or_a_restart_between_reads_as_p_nsr_says` |
//! | Unmapped addresses | RM 3 lists what exists; an address it does not list reads zero and takes no write | RM 3 | `every_register_reads_and_writes_at_its_own_address_and_bursts_increment` |
//! | What it measures | Angular rate in thousandths of a degree per second, specific force in millionths of a gravity, temperature in hundredths of a degree, on the unit's own axes, PS figure 4; [`Mpu9250::set_rate`], [`Mpu9250::set_acceleration`], [`Mpu9250::set_temperature`]; the field is [`Mpu9250::set_field`] on the package's axes, turned onto the die's, figure 5 against figure 4, and reaches [`Ak8963::set_field`] through [`Mpu9250::magnetometer`] | PS 9.1 | `the_words_are_the_signals_at_the_selected_range`, `sensing_puts_the_field_on_the_compass_die_s_own_axes` |
//!
//! # What is not, and why
//!
//! | Departure | Reason |
//! |---|---|
//! | A signal changing between two reads | Every sample not yet materialised is generated at the read from the value held then, so the samples either side of the change do not differ; a driver averaging eight into a millisecond cannot tell. The accelerometer's 4 kHz against the gyro's 8 kHz — "every 2nd accel sample is a duplicate", as the driver puts it — is the same rule, and comes out the same |
//! | The master running once per slave per read | The part runs each enabled slave at every sample; the model runs it once for the samples that elapsed, since only the last result is in the registers. The delay rule is still counted per sample |
//! | `BYTE_SW`, `GRP`, `WAIT_FOR_ES`, `MULT_MST_EN`, `DELAY_ES_SHADOW`, the master clock divider | Stored; the driver sets none of them. Swapping bytes would be a line, and is not written without a reader |
//! | External sensor data in the FIFO, `SLV_x` bits of `FIFO_EN` and `SLV_3_FIFO_EN` | Stored; the driver keeps the slaves out of its FIFO |
//! | The digital low-pass filters, self-test, wake-on-motion, `LP_ACCEL_ODR`, `CYCLE`, `GYRO_STANDBY`, `FSYNC`, `EXT_SYNC_SET` | Stored. A filter's bandwidth is nothing to a signal a plant holds |
//! | The `INT` pin | Not driven; on this board it is an input the driver never reads |
//! | Start-up time from sleep, 35 ms for the gyroscope and 30 ms for the accelerometer | PS 3.1 and 3.2; data is valid from the first sample. The driver waits 100 ms after its reset |
//! | The clock ceilings of 1 MHz and 20 MHz on `SCLK` | The seam carries words, not a clock |
//! | Writes to `FIFO_R_W` | "Read/Write command" — a write would push a byte; the driver never does, and it is dropped |
use crate::chip::ak8963::Ak8963;
use crate::chip::i2c;
use crate::chip::invensense::{self, Access, Fifo, SampleClock, Serial, Signals};
use crate::chip::spi::Peer;
use crate::device::DeviceContext;
use crate::time::VirtualNanos;
/// RM 3's register map, by address.
const XG_OFFSET_H: usize = 0x13;
const SMPLRT_DIV: usize = 0x19;
const CONFIG: usize = 0x1A;
const GYRO_CONFIG: usize = 0x1B;
const ACCEL_CONFIG: usize = 0x1C;
const ACCEL_CONFIG_2: usize = 0x1D;
const FIFO_EN: usize = 0x23;
const I2C_MST_CTRL: usize = 0x24;
const I2C_SLV0_ADDR: usize = 0x25;
const I2C_SLV4_ADDR: usize = 0x31;
const I2C_SLV4_REG: usize = 0x32;
const I2C_SLV4_DO: usize = 0x33;
const I2C_SLV4_CTRL: usize = 0x34;
const I2C_SLV4_DI: usize = 0x35;
const I2C_MST_STATUS: usize = 0x36;
const INT_PIN_CFG: usize = 0x37;
const INT_STATUS: usize = 0x3A;
const ACCEL_XOUT_H: usize = 0x3B;
const GYRO_ZOUT_L: usize = 0x48;
const EXT_SENS_DATA_00: usize = 0x49;
const I2C_SLV0_DO: usize = 0x63;
const I2C_MST_DELAY_CTRL: usize = 0x67;
const USER_CTRL: usize = 0x6A;
const PWR_MGMT_1: usize = 0x6B;
const PWR_MGMT_2: usize = 0x6C;
const FIFO_COUNTH: usize = 0x72;
const FIFO_COUNTL: usize = 0x73;
const WHO_AM_I: usize = 0x75;
const XA_OFFSET_H: usize = 0x77;
const REGISTERS: usize = 0x80;
/// The bits the model acts on, by register, RM 4.
const DEVICE_ID: u8 = 0x71;
const H_RESET: u8 = 0x80;
const SLEEP: u8 = 0x40;
const CLKSEL: u8 = 0x07;
const CLOCK_STOPPED: u8 = 0x07;
const FIFO_MODE: u8 = 0x40;
const DLPF_CFG: u8 = 0x07;
const FS_SEL: u8 = 0x18;
const FCHOICE_B: u8 = 0x03;
const I2C_MST_P_NSR: u8 = 0x10;
const I2C_SLV_RNW: u8 = 0x80;
const I2C_SLV_EN: u8 = 0x80;
const I2C_SLV_REG_DIS: u8 = 0x20;
const I2C_SLV_LENG: u8 = 0x0F;
const I2C_MST_DLY: u8 = 0x1F;
const I2C_SLV4_DONE: u8 = 0x40;
const INT_ANYRD_2CLEAR: u8 = 0x10;
const FIFO_OVERFLOW_INT: u8 = 0x10;
const RAW_DATA_RDY_INT: u8 = 0x01;
const USER_FIFO_EN: u8 = 0x40;
const I2C_MST_EN: u8 = 0x20;
const I2C_IF_DIS: u8 = 0x10;
const FIFO_RST: u8 = 0x04;
const I2C_MST_RST: u8 = 0x02;
const SIG_COND_RST: u8 = 0x01;
const AUTO_CLEARING: u8 = I2C_IF_DIS | FIFO_RST | I2C_MST_RST | SIG_COND_RST;
const DISABLE_ACCEL: [u8; 3] = [0x20, 0x10, 0x08];
const DISABLE_GYRO: [u8; 3] = [0x04, 0x02, 0x01];
/// PS 4.17: "a 512-byte FIFO register".
pub const FIFO_BYTES: usize = invensense::FIFO_BYTES;
/// RM 4.5's table: the two internal sample rates.
const FAST_HZ: u64 = 8_000;
const SLOW_HZ: u64 = 1_000;
/// PS 3.4.2: 333.87 LSB per degree and no offset at 21 °C, as hundredths and a ten-thousandth.
const TEMP_HUNDREDTHS_PER_DEGREE: i64 = 33_387;
const TEMP_ZERO_CENTI: i32 = 2_100;
/// RM 4.25: twenty-four external sensor data registers.
const EXTERNAL_BYTES: usize = 24;
/// One slave's transaction as its registers describe it, RM 4.13 and 4.17.
#[derive(Debug, Clone, Copy)]
struct Transfer {
/// `I2C_SLVx_ADDR`: the direction in bit 7, the address below it.
address: u8,
register: u8,
skip_register: bool,
length: usize,
out: u8,
stop_between: bool,
}
/// Slaves 0 to 3 of the master, RM 4.13 to 4.16: a slave's place in the external data registers.
#[derive(Debug, Clone, Copy, Default)]
struct Allocation {
/// Offset and length in `EXT_SENS_DATA`, or `None` for a slave not allocated.
slots: [Option<(usize, usize)>; 4],
next: usize,
held: bool,
}
/// One MPU-9250, with its magnetometer die and whatever else a board puts on its auxiliary bus.
#[derive(Debug)]
pub struct Mpu9250 {
registers: [u8; REGISTERS],
signals: Signals,
clock: SampleClock,
fifo: Fifo,
last_byte: u8,
allocation: Allocation,
magnetometer: Ak8963,
others: Vec<Box<dyn i2c::Peer>>,
serial: Serial,
}
impl Default for Mpu9250 {
fn default() -> Self {
Self::new()
}
}
impl Mpu9250 {
/// A unit at power-on: the registers at RM 3's reset values, the FIFO empty, the magnetometer die on
/// its bus, and the signals a still, level unit at 21 °C gives — no rate, one gravity on Z.
#[must_use]
pub fn new() -> Self {
let mut unit = Self {
registers: [0; REGISTERS],
signals: Signals::still(TEMP_ZERO_CENTI),
clock: SampleClock::new(),
fifo: Fifo::new(),
last_byte: 0,
allocation: Allocation::default(),
magnetometer: Ak8963::new(),
others: Vec::new(),
serial: Serial::new(),
};
unit.reset(VirtualNanos(0));
unit
}
/// The angular rate the unit measures from now on, in thousandths of a degree per second on its axes.
pub fn set_rate(&mut self, x: i32, y: i32, z: i32) {
self.signals.rate = [x, y, z];
}
/// The specific force the unit measures from now on, in millionths of a gravity on its axes.
pub fn set_acceleration(&mut self, x: i32, y: i32, z: i32) {
self.signals.acceleration = [x, y, z];
}
/// The die temperature from now on, in hundredths of a degree.
pub fn set_temperature(&mut self, centi_celsius: i32) {
self.signals.centi_celsius = centi_celsius;
}
/// The magnetic field from now on, in nanotesla on the package's axes: the die inside sits turned
/// against them, PS figure 5 against figure 4, so the package's `y` is the die's `x`, the package's
/// `x` the die's `y`, and the package's `z` against the die's.
pub fn set_field(&mut self, x: i32, y: i32, z: i32) {
self.magnetometer.set_field(y, x, -z);
}
/// The magnetometer die on the package's own bus, to drive the field it measures.
pub fn magnetometer(&mut self) -> &mut Ak8963 {
&mut self.magnetometer
}
/// Hangs another chip on the auxiliary bus beside the die, which is what a board with a sensor on
/// `AUX_DA` and `AUX_CL` does.
pub fn attach_auxiliary(&mut self, peer: impl i2c::Peer + 'static) {
self.others.push(Box::new(peer));
}
/// RM 3's reset values, the FIFO emptied, sampling restarted.
fn reset(&mut self, now: VirtualNanos) {
self.registers = [0; REGISTERS];
self.registers[PWR_MGMT_1] = 0x01;
self.registers[WHO_AM_I] = DEVICE_ID;
self.fifo.clear();
self.last_byte = 0;
self.allocation = Allocation::default();
self.clock.restart(now);
}
/// RM 4.5's table and 4.4: the sample rate the registers now give, in hertz, or nothing while the
/// clock is stopped or the unit asleep.
fn sample_hz(&self) -> u64 {
let power = self.registers[PWR_MGMT_1];
if power & SLEEP != 0 || power & CLKSEL == CLOCK_STOPPED {
return 0;
}
let dlpf = self.registers[CONFIG] & DLPF_CFG;
if self.registers[GYRO_CONFIG] & FCHOICE_B != 0 || dlpf == 0 || dlpf == 7 {
FAST_HZ
} else {
SLOW_HZ / (1 + u64::from(self.registers[SMPLRT_DIV]))
}
}
/// A change of rate or of running: the samples so far stand, the next come at the new rate.
fn rebase(&mut self, now: VirtualNanos) {
self.clock.rebase(now, self.sample_hz());
}
/// Bits 4:3 of a configuration register, the range step.
fn range_step(register: u8) -> u32 {
u32::from((register & FS_SEL) >> 3)
}
/// The fourteen bytes of one sample, registers 59 to 72 in order, from the signals as held.
fn sample(&self) -> [u8; invensense::SAMPLE_BYTES] {
let accel_step = Self::range_step(self.registers[ACCEL_CONFIG]);
let gyro_step = Self::range_step(self.registers[GYRO_CONFIG]);
let mut accel = [0; 3];
let mut gyro = [0; 3];
for axis in 0..3 {
if self.registers[PWR_MGMT_2] & DISABLE_ACCEL[axis] == 0 {
accel[axis] = self
.signals
.accel_counts(axis, accel_step, self.accel_offset(axis));
}
if self.registers[PWR_MGMT_2] & DISABLE_GYRO[axis] == 0 {
let offset = i64::from(i16::from_be_bytes([
self.registers[XG_OFFSET_H + 2 * axis],
self.registers[XG_OFFSET_H + 2 * axis + 1],
]));
gyro[axis] =
self.signals.gyro_counts(axis, gyro_step) - ((offset * 4) >> gyro_step);
}
}
let temperature = invensense::round(
i64::from(self.signals.centi_celsius - TEMP_ZERO_CENTI) * TEMP_HUNDREDTHS_PER_DEGREE,
10_000,
);
invensense::pack(accel, temperature, gyro)
}
/// RM 4.39: fifteen bits, `XA_OFFS[14:7]` in the high register and `[6:0]` in bits 7:1 of the low.
fn accel_offset(&self, axis: usize) -> i64 {
invensense::offset15(
self.registers[XA_OFFSET_H + 3 * axis],
self.registers[XA_OFFSET_H + 3 * axis + 1],
)
}
/// Everything that happened between the last select and `now`: the samples that fell due land in the
/// data registers and the FIFO, and the master runs the slaves that were due.
fn catch_up(&mut self, now: VirtualNanos, context: &DeviceContext<'_>) {
let taken = self.clock.catch_up(now, self.sample_hz());
if taken.is_empty() {
return;
}
let sample = self.sample();
self.registers[ACCEL_XOUT_H..=GYRO_ZOUT_L].copy_from_slice(&sample);
self.registers[INT_STATUS] |= RAW_DATA_RDY_INT;
if self.registers[USER_CTRL] & USER_FIFO_EN != 0 {
let bytes = invensense::fifo_bytes(&sample, self.registers[FIFO_EN]);
let keep_oldest = self.registers[CONFIG] & FIFO_MODE != 0;
if self.fifo.push(&bytes, taken.end - taken.start, keep_oldest) {
self.registers[INT_STATUS] |= FIFO_OVERFLOW_INT;
}
}
if self.registers[USER_CTRL] & I2C_MST_EN != 0 {
self.run_master(taken.start + 1..=taken.end, context);
}
}
/// RM 4.25 and 4.30: which of slaves 0 to 3 fell due over the samples `cycles`, then each once.
fn run_master(&mut self, cycles: std::ops::RangeInclusive<u64>, context: &DeviceContext<'_>) {
let delay = u64::from(self.registers[I2C_SLV4_CTRL] & I2C_MST_DLY) + 1;
let delayed = self.registers[I2C_MST_DELAY_CTRL];
let p_nsr = self.registers[I2C_MST_CTRL] & I2C_MST_P_NSR != 0;
for slave in 0..4 {
let control = self.registers[I2C_SLV0_ADDR + 3 * slave + 2];
let length = usize::from(control & I2C_SLV_LENG);
if control & I2C_SLV_EN == 0 || length == 0 {
continue;
}
let runs = if delayed & (1 << slave) != 0 {
cycles.clone().any(|cycle| cycle % delay == 0)
} else {
true
};
if !runs {
continue;
}
let slot = self.allocate(slave, length);
let transfer = Transfer {
address: self.registers[I2C_SLV0_ADDR + 3 * slave],
register: self.registers[I2C_SLV0_ADDR + 3 * slave + 1],
skip_register: control & I2C_SLV_REG_DIS != 0,
length,
out: self.registers[I2C_SLV0_DO + slave],
stop_between: p_nsr,
};
match self.transaction(transfer, context) {
Some(data) => {
if let Some((offset, _)) = slot {
for (index, byte) in data.iter().enumerate() {
if offset + index < EXTERNAL_BYTES {
self.registers[EXT_SENS_DATA_00 + offset + index] = *byte;
}
}
}
}
None => self.registers[I2C_MST_STATUS] |= 1 << slave,
}
}
let control = self.registers[I2C_SLV4_CTRL];
if control & I2C_SLV_EN != 0 {
let transfer = Transfer {
address: self.registers[I2C_SLV4_ADDR],
register: self.registers[I2C_SLV4_REG],
skip_register: control & I2C_SLV_REG_DIS != 0,
length: 1,
out: self.registers[I2C_SLV4_DO],
stop_between: p_nsr,
};
match self.transaction(transfer, context) {
Some(data) => {
if let Some(byte) = data.first() {
self.registers[I2C_SLV4_DI] = *byte;
}
}
None => self.registers[I2C_MST_STATUS] |= 1 << 4,
}
self.registers[I2C_SLV4_CTRL] &= !I2C_SLV_EN;
self.registers[I2C_MST_STATUS] |= I2C_SLV4_DONE;
}
}
/// RM 4.25: a slave's registers, "in order of slave number, starting from `EXT_SENS_DATA_00`", held
/// until every slave is disabled or the master is reset.
fn allocate(&mut self, slave: usize, length: usize) -> Option<(usize, usize)> {
if !self.allocation.held {
self.allocation = Allocation {
held: true,
..Allocation::default()
};
}
if self.allocation.slots[slave].is_none() {
let offset = self.allocation.next;
self.allocation.slots[slave] = Some((offset, length));
self.allocation.next += length;
}
self.allocation.slots[slave]
}
/// One transaction on the auxiliary bus, RM 4.13: the address written with the register unless
/// `REG_DIS`, then `length` bytes read after a restart or a stop and a start, or one byte written.
/// `None` where no peer acknowledged the address.
fn transaction(&mut self, transfer: Transfer, context: &DeviceContext<'_>) -> Option<Vec<u8>> {
let Transfer {
address,
register,
skip_register,
length,
out,
stop_between,
} = transfer;
let read = address & I2C_SLV_RNW != 0;
let address = address & 0x7F;
let Self {
magnetometer,
others,
..
} = self;
let mut peers: Vec<&mut dyn i2c::Peer> = vec![magnetometer];
peers.extend(others.iter_mut().map(Box::as_mut));
let mut answering = None;
for peer in peers {
if peer.start(address, read && skip_register, context) {
answering = Some(peer);
break;
}
}
let peer = answering?;
if !skip_register {
peer.write(register, context);
}
if !read {
peer.write(out, context);
peer.stop(context);
return Some(Vec::new());
}
if !skip_register {
if stop_between {
peer.stop(context);
}
peer.start(address, true, context);
}
let data: Vec<u8> = (0..length)
.map(|index| peer.read(index + 1 == length, context))
.collect();
peer.stop(context);
Some(data)
}
/// RM 3's table: whether an address is a register at all.
fn mapped(register: usize) -> bool {
matches!(
register,
0x00..=0x02
| 0x0D..=0x0F
| 0x13..=0x1F
| 0x23..=0x38
| 0x3A..=0x60
| 0x63..=0x6C
| 0x72..=0x75
| 0x77
| 0x78
| 0x7A
| 0x7B
| 0x7D
| 0x7E
)
}
/// RM 3's "Serial I/F" column: the registers a write lands in.
fn writable(register: usize) -> bool {
Self::mapped(register)
&& !matches!(
register,
I2C_SLV4_DI | I2C_MST_STATUS | INT_STATUS | ACCEL_XOUT_H
..=0x60 | FIFO_COUNTH | FIFO_COUNTL | WHO_AM_I
)
}
fn read_register(&mut self, register: usize) -> u8 {
let value = match register {
FIFO_COUNTH => self.fifo.latch(),
FIFO_COUNTL => self.fifo.latched_low(),
invensense::FIFO_R_W => {
if let Some(byte) = self.fifo.pop() {
self.last_byte = byte;
}
self.last_byte
}
INT_STATUS => {
let value = self.registers[INT_STATUS];
self.registers[INT_STATUS] = 0;
value
}
I2C_MST_STATUS => {
let value = self.registers[I2C_MST_STATUS];
self.registers[I2C_MST_STATUS] = 0;
value
}
// Every unmapped address reads zero because no write lands in one; RM 3.
_ => self.registers[register],
};
if self.registers[INT_PIN_CFG] & INT_ANYRD_2CLEAR != 0 {
self.registers[INT_STATUS] = 0;
}
value
}
fn write_register(&mut self, register: usize, byte: u8, now: VirtualNanos) {
if !Self::writable(register) {
return;
}
match register {
PWR_MGMT_1 => {
if byte & H_RESET != 0 {
self.reset(now);
return;
}
self.rebase(now);
self.registers[PWR_MGMT_1] = byte;
}
USER_CTRL => {
self.registers[USER_CTRL] = byte & !AUTO_CLEARING;
if byte & FIFO_RST != 0 {
self.fifo.clear();
}
if byte & I2C_MST_RST != 0 {
self.allocation.held = false;
}
if byte & SIG_COND_RST != 0 {
for data in ACCEL_XOUT_H..=GYRO_ZOUT_L {
self.registers[data] = 0;
}
}
}
SMPLRT_DIV | CONFIG | GYRO_CONFIG | ACCEL_CONFIG_2 => {
self.rebase(now);
self.registers[register] = byte;
}
_ => {
self.registers[register] = byte;
if (I2C_SLV0_ADDR..I2C_SLV4_ADDR).contains(®ister)
&& (register - I2C_SLV0_ADDR) % 3 == 2
&& (0..4).all(|slave| {
self.registers[I2C_SLV0_ADDR + 3 * slave + 2] & I2C_SLV_EN == 0
})
{
self.allocation.held = false;
}
}
}
}
}
impl Peer for Mpu9250 {
/// The select falling: everything due by now happens, then the first byte is an address.
fn select(&mut self, context: &DeviceContext<'_>) {
self.serial.select();
self.catch_up(context.now(), context);
}
fn deselect(&mut self, _context: &DeviceContext<'_>) {
self.serial.deselect();
}
/// PS 7.5: the address byte with its direction bit, then a byte per register, incrementing; PS 4.17:
/// "The FIFO register supports burst reads" — a burst stays on it.
fn transfer(&mut self, mosi: u16, context: &DeviceContext<'_>) -> u16 {
match self.serial.step((mosi & 0xFF) as u8) {
Some(Access::Read(register)) => u16::from(self.read_register(register)),
Some(Access::Write(register, byte)) => {
self.write_register(register, byte, context.now());
0
}
None => 0,
}
}
}