311 lines
11 KiB
Rust
311 lines
11 KiB
Rust
//! ICM-42688-P register protocol and asynchronous sample driver.
|
|
|
|
mod error;
|
|
mod sample;
|
|
pub mod service;
|
|
|
|
use kaptan_sensor::imu::ImuSnapshot;
|
|
use kaptan_spi::{SpiCompletion, SpiDevice};
|
|
use kaptan_time::Timestamp;
|
|
|
|
pub use error::Error;
|
|
pub use service::{Icm42688Service, Icm42688ServiceEvent};
|
|
|
|
const WHO_AM_I: u8 = 0x75;
|
|
const EXPECTED_WHO_AM_I: u8 = 0x47;
|
|
const DEVICE_CONFIG: u8 = 0x11;
|
|
const SOFT_RESET: u8 = 0x01;
|
|
const PWR_MGMT0: u8 = 0x4e;
|
|
const GYRO_CONFIG0: u8 = 0x4f;
|
|
const ACCEL_CONFIG0: u8 = 0x50;
|
|
const GYRO_ACCEL_CONFIG0: u8 = 0x52;
|
|
const INT_CONFIG: u8 = 0x14;
|
|
const INT_CONFIG0: u8 = 0x63;
|
|
const INT_CONFIG1: u8 = 0x64;
|
|
const INT_SOURCE0: u8 = 0x65;
|
|
const INTF_CONFIG1: u8 = 0x4d;
|
|
const TEMP_DATA1: u8 = 0x1d;
|
|
const SAMPLE_BYTES: usize = 14;
|
|
const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
|
|
|
|
// SPI mode 0, active-high push-pull, pulsed INT1. Data-ready is cleared by
|
|
// the SPI burst read and its source is explicitly enabled on INT1.
|
|
const CONFIGURATION: [(u8, u8); 9] = [
|
|
(INT_CONFIG, 0x03),
|
|
(INT_CONFIG0, 0x00),
|
|
(INT_CONFIG1, 0x60),
|
|
(INTF_CONFIG1, 0x40),
|
|
(GYRO_ACCEL_CONFIG0, 0xff),
|
|
(INT_SOURCE0, 0x08),
|
|
(PWR_MGMT0, 0x0f),
|
|
// ±500°/s and ±4 g at 1 kHz. The two range fields use the ICM-42688-P
|
|
// encoding in bits 7:5 and the output data rate is in bits 3:0.
|
|
(GYRO_CONFIG0, 0x46),
|
|
(ACCEL_CONFIG0, 0x46),
|
|
];
|
|
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
enum State {
|
|
Dormant,
|
|
Resetting,
|
|
WaitingForReset,
|
|
ReadingIdentity,
|
|
WritingConfiguration(usize),
|
|
WaitingForSensorStart,
|
|
Ready,
|
|
ReadingSample { timestamp: Timestamp },
|
|
}
|
|
|
|
/// Observable outcome of one ICM-42688-P SPI terminal event.
|
|
#[derive(Debug, Eq, PartialEq)]
|
|
pub enum Icm42688Event<BusError> {
|
|
/// The selected SPI interrupt had no terminal transfer event.
|
|
NotPending,
|
|
/// One initialization transfer completed; initialization continues.
|
|
Initializing,
|
|
/// The ICM-42688-P identity and configuration sequence completed.
|
|
Initialized,
|
|
/// One coherent IMU sample completed.
|
|
Sample(ImuSnapshot),
|
|
/// Initialization or acquisition failed.
|
|
Fault(Error<BusError>),
|
|
}
|
|
|
|
/// Configured ICM-42688-P driver.
|
|
///
|
|
/// The driver owns fixed transfer buffers and progresses exclusively from
|
|
/// timer, data-ready and SPI-DMA completion events. It never waits for a
|
|
/// peripheral status flag.
|
|
pub struct Icm42688<Bus> {
|
|
bus: Bus,
|
|
state: State,
|
|
transmit: [u8; TRANSFER_CAPACITY],
|
|
receive: [u8; TRANSFER_CAPACITY],
|
|
}
|
|
|
|
impl<Bus> Icm42688<Bus> {
|
|
/// Creates an inactive ICM-42688-P state machine.
|
|
pub const fn new(bus: Bus) -> Self {
|
|
Self {
|
|
bus,
|
|
state: State::Dormant,
|
|
transmit: [0; TRANSFER_CAPACITY],
|
|
receive: [0; TRANSFER_CAPACITY],
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<Bus: SpiDevice> Icm42688<Bus> {
|
|
/// Starts the sensor reset. A subsequent periodic tick provides the
|
|
/// datasheet-required reset-settling interval before identification.
|
|
pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
|
|
if self.state != State::Dormant {
|
|
return Err(Error::NotReady);
|
|
}
|
|
self.prepare_write(DEVICE_CONFIG, SOFT_RESET);
|
|
self.submit(2)?;
|
|
self.state = State::Resetting;
|
|
Ok(())
|
|
}
|
|
|
|
/// Advances reset/wake delays from the board periodic event.
|
|
pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
|
|
match self.state {
|
|
State::WaitingForReset => {
|
|
self.prepare_read(WHO_AM_I, 2);
|
|
self.submit(2)?;
|
|
self.state = State::ReadingIdentity;
|
|
}
|
|
State::WaitingForSensorStart => {
|
|
self.start_configuration(7)?;
|
|
}
|
|
_ => {}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
/// Starts one temperature, accelerometer and gyroscope burst read.
|
|
pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
|
if self.state != State::Ready {
|
|
return Err(Error::NotReady);
|
|
}
|
|
self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY);
|
|
self.submit(TRANSFER_CAPACITY)?;
|
|
self.state = State::ReadingSample { timestamp };
|
|
Ok(())
|
|
}
|
|
|
|
/// Advances the state machine from an already-acknowledged SPI completion.
|
|
pub fn on_transfer_complete(
|
|
&mut self,
|
|
completion: SpiCompletion,
|
|
) -> Icm42688Event<Bus::StartError> {
|
|
self.bus.complete_transfer(completion);
|
|
match completion {
|
|
SpiCompletion::NotPending => Icm42688Event::NotPending,
|
|
SpiCompletion::TransferFault => {
|
|
self.state = State::Dormant;
|
|
Icm42688Event::Fault(Error::TransferFault)
|
|
}
|
|
SpiCompletion::Complete => self.advance_after_completion(),
|
|
}
|
|
}
|
|
|
|
/// Returns whether an IMU data-ready event can start a sensor transfer.
|
|
pub fn is_ready(&self) -> bool {
|
|
self.state == State::Ready
|
|
}
|
|
|
|
fn advance_after_completion(&mut self) -> Icm42688Event<Bus::StartError> {
|
|
match self.state {
|
|
State::Dormant => Icm42688Event::NotPending,
|
|
State::Resetting => {
|
|
self.state = State::WaitingForReset;
|
|
Icm42688Event::Initializing
|
|
}
|
|
State::WaitingForReset | State::WaitingForSensorStart => Icm42688Event::NotPending,
|
|
State::ReadingIdentity => {
|
|
let identity = self.receive[1];
|
|
if identity != EXPECTED_WHO_AM_I {
|
|
self.state = State::Dormant;
|
|
return Icm42688Event::Fault(Error::UnexpectedIdentity(identity));
|
|
}
|
|
self.start_configuration_event(0)
|
|
}
|
|
State::WritingConfiguration(index) => {
|
|
let next = index + 1;
|
|
if next == 7 {
|
|
// PWR_MGMT0 starts both sensor paths. The following
|
|
// timer tick gives them more than the required 200 µs
|
|
// before their output-rate configuration is written.
|
|
self.state = State::WaitingForSensorStart;
|
|
Icm42688Event::Initializing
|
|
} else if next == CONFIGURATION.len() {
|
|
self.state = State::Ready;
|
|
Icm42688Event::Initialized
|
|
} else {
|
|
self.start_configuration_event(next)
|
|
}
|
|
}
|
|
State::Ready => Icm42688Event::NotPending,
|
|
State::ReadingSample { timestamp } => {
|
|
let mut bytes = [0; SAMPLE_BYTES];
|
|
bytes.copy_from_slice(&self.receive[1..]);
|
|
self.state = State::Ready;
|
|
Icm42688Event::Sample(sample::decode(&bytes, timestamp))
|
|
}
|
|
}
|
|
}
|
|
|
|
fn start_configuration_event(&mut self, index: usize) -> Icm42688Event<Bus::StartError> {
|
|
match self.start_configuration(index) {
|
|
Ok(()) => Icm42688Event::Initializing,
|
|
Err(error) => Icm42688Event::Fault(error),
|
|
}
|
|
}
|
|
|
|
fn start_configuration(&mut self, index: usize) -> Result<(), Error<Bus::StartError>> {
|
|
let (register, value) = CONFIGURATION[index];
|
|
self.prepare_write(register, value);
|
|
if let Err(error) = self.submit(2) {
|
|
self.state = State::Dormant;
|
|
return Err(error);
|
|
}
|
|
self.state = State::WritingConfiguration(index);
|
|
Ok(())
|
|
}
|
|
|
|
fn prepare_read(&mut self, register: u8, length: usize) {
|
|
self.transmit = [0; TRANSFER_CAPACITY];
|
|
self.receive = [0; TRANSFER_CAPACITY];
|
|
self.transmit[0] = register | 0x80;
|
|
debug_assert!(length <= TRANSFER_CAPACITY);
|
|
}
|
|
|
|
fn prepare_write(&mut self, register: u8, value: u8) {
|
|
self.transmit = [0; TRANSFER_CAPACITY];
|
|
self.receive = [0; TRANSFER_CAPACITY];
|
|
self.transmit[..2].copy_from_slice(&[register, value]);
|
|
}
|
|
|
|
fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> {
|
|
let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive);
|
|
bus.start_transfer(&transmit[..length], &mut receive[..length])
|
|
.map_err(Error::Start)
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::{Error, Icm42688, Icm42688Event, TRANSFER_CAPACITY};
|
|
use kaptan_spi::{SpiCompletion, SpiDevice};
|
|
use kaptan_time::Timestamp;
|
|
|
|
struct Bus {
|
|
response: [u8; TRANSFER_CAPACITY],
|
|
}
|
|
|
|
impl SpiDevice for Bus {
|
|
type StartError = ();
|
|
|
|
fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
|
|
receive.copy_from_slice(&self.response[..receive.len()]);
|
|
Ok(())
|
|
}
|
|
|
|
fn complete_transfer(&mut self, _: SpiCompletion) {}
|
|
}
|
|
|
|
#[test]
|
|
fn accepts_a_sample_only_after_identity_and_wake_delays() {
|
|
let mut response = [0; TRANSFER_CAPACITY];
|
|
response[1] = 0x47;
|
|
let mut imu = Icm42688::new(Bus { response });
|
|
imu.begin_initialization().unwrap();
|
|
assert!(matches!(
|
|
imu.on_transfer_complete(SpiCompletion::Complete),
|
|
Icm42688Event::Initializing
|
|
));
|
|
assert!(matches!(imu.advance_initialization(), Ok(())));
|
|
assert!(matches!(
|
|
imu.on_transfer_complete(SpiCompletion::Complete),
|
|
Icm42688Event::Initializing
|
|
));
|
|
for _ in 0..7 {
|
|
assert!(matches!(
|
|
imu.on_transfer_complete(SpiCompletion::Complete),
|
|
Icm42688Event::Initializing
|
|
));
|
|
}
|
|
assert!(matches!(imu.advance_initialization(), Ok(())));
|
|
assert!(matches!(
|
|
imu.on_transfer_complete(SpiCompletion::Complete),
|
|
Icm42688Event::Initializing
|
|
));
|
|
assert!(matches!(
|
|
imu.on_transfer_complete(SpiCompletion::Complete),
|
|
Icm42688Event::Initialized
|
|
));
|
|
assert!(imu.is_ready());
|
|
imu.request_sample(Timestamp::from_microseconds(1)).unwrap();
|
|
assert!(matches!(
|
|
imu.on_transfer_complete(SpiCompletion::Complete),
|
|
Icm42688Event::Sample(_)
|
|
));
|
|
}
|
|
|
|
#[test]
|
|
fn rejects_a_wrong_identity() {
|
|
let mut imu = Icm42688::new(Bus {
|
|
response: [0; TRANSFER_CAPACITY],
|
|
});
|
|
imu.begin_initialization().unwrap();
|
|
let _ = imu.on_transfer_complete(SpiCompletion::Complete);
|
|
imu.advance_initialization().unwrap();
|
|
assert!(matches!(
|
|
imu.on_transfer_complete(SpiCompletion::Complete),
|
|
Icm42688Event::Fault(Error::UnexpectedIdentity(0))
|
|
));
|
|
}
|
|
}
|