feat(drivers): validate bounded IMU FIFO acquisition
Drain bounded MPU6000 and ICM-42688 FIFO batches. Publish the latest validated sample only after overflow checks. Reject invalid drains. Validation: driver, sensor and platform tests plus both ARM release builds from a fresh build directory.
This commit is contained in:
Onur Özkan committed
1 parent
f9a61a086f
commit
d32b03c87f
13 files changed
+1303
-833
No files matched your search
@@ -27,7 +27,19 @@ pub enum Error<BusError> {
|
||||
/// The bus stayed busy for the pending operation's 20 ms admission budget.
|
||||
#[kaptan_error(id = 5107)]
|
||||
AdmissionTimeout,
|
||||
/// No acquisition was requested within two output periods plus 10 ms.
|
||||
/// No checked, nonempty batch arrived within two output periods plus 10 ms.
|
||||
#[kaptan_error(id = 5108)]
|
||||
DataReadyTimeout,
|
||||
/// FIFO full status or the lost-packet counter reports missing records.
|
||||
#[kaptan_error(id = 5109)]
|
||||
FifoOverflow,
|
||||
/// The reported FIFO count exceeds the configured packet capacity.
|
||||
#[kaptan_error(id = 5110)]
|
||||
FifoCount(u16),
|
||||
/// A packet has an invalid header, sensor code, or timestamp order.
|
||||
#[kaptan_error(id = 5111)]
|
||||
FifoPacket,
|
||||
/// The sensor reset after its configuration had completed.
|
||||
#[kaptan_error(id = 5112)]
|
||||
UnexpectedReset,
|
||||
}
|
||||
File diff suppressed because it is too large.
Load diff
@@ -1,119 +1,158 @@
|
||||
//! ICM-42688-P burst-sample decoding.
|
||||
//!
|
||||
//! Integer division rounds toward zero before adding temperature's offset.
|
||||
//! Over the full signed raw domain, the configured ranges decode to
|
||||
//! -4,000..=3,999 mg, -8,731..=8,731 mrad/s and -22,234..=27,233 cdegC.
|
||||
//! Products fit i32 and the acceleration and temperature results fit i16.
|
||||
//! ICM-42688-P 16-byte FIFO packet decoding.
|
||||
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
use super::SAMPLE_BYTES;
|
||||
|
||||
pub(crate) fn decode(bytes: &[u8; SAMPLE_BYTES], timestamp: Timestamp) -> ImuSnapshot {
|
||||
ImuSnapshot::new(
|
||||
timestamp,
|
||||
[
|
||||
scale_acceleration(read_i16(bytes, 2)),
|
||||
scale_acceleration(read_i16(bytes, 4)),
|
||||
scale_acceleration(read_i16(bytes, 6)),
|
||||
],
|
||||
[
|
||||
scale_angular_rate(read_i16(bytes, 8)),
|
||||
scale_angular_rate(read_i16(bytes, 10)),
|
||||
scale_angular_rate(read_i16(bytes, 12)),
|
||||
],
|
||||
scale_temperature(read_i16(bytes, 0)),
|
||||
)
|
||||
pub(crate) fn decode(
|
||||
bytes: &[u8; SAMPLE_BYTES],
|
||||
timestamp: Timestamp,
|
||||
previous_tick: Option<u16>,
|
||||
) -> Option<(ImuSnapshot, u16)> {
|
||||
// Packet 3, both sensors, ordinary resolution, ODR timestamp. Only the
|
||||
// first packet after explicit reset/configuration may report ODR changes.
|
||||
if bytes[0] & 0xfc != 0x68 || (previous_tick.is_some() && bytes[0] != 0x68) {
|
||||
return None;
|
||||
}
|
||||
// InvenSense eMD Icm426xx: ordinary FIFO temperature 0x80 is invalid.
|
||||
if bytes[13] == 0x80 {
|
||||
return None;
|
||||
}
|
||||
let tick = u16::from_be_bytes([bytes[14], bytes[15]]);
|
||||
if let Some(previous) = previous_tick {
|
||||
let delta = tick.wrapping_sub(previous);
|
||||
// Ordering only: internal-clock ticks are not microseconds (§12.7).
|
||||
// This does not establish sample spacing or exclude multiple wraps.
|
||||
if delta == 0 || delta >= 0x8000 {
|
||||
return None;
|
||||
}
|
||||
}
|
||||
let raw = [
|
||||
read_i16(bytes, 1),
|
||||
read_i16(bytes, 3),
|
||||
read_i16(bytes, 5),
|
||||
read_i16(bytes, 7),
|
||||
read_i16(bytes, 9),
|
||||
read_i16(bytes, 11),
|
||||
];
|
||||
// Invalid-code mode reserves -32768; valid readings below -32766 clamp
|
||||
// to -32766 in the sensor (DS-000347 §12.8).
|
||||
if raw.iter().any(|value| *value < -32766) {
|
||||
return None;
|
||||
}
|
||||
Some((
|
||||
ImuSnapshot::new(
|
||||
timestamp,
|
||||
[
|
||||
scale_acceleration(raw[0]),
|
||||
scale_acceleration(raw[1]),
|
||||
scale_acceleration(raw[2]),
|
||||
],
|
||||
[
|
||||
scale_angular_rate(raw[3]),
|
||||
scale_angular_rate(raw[4]),
|
||||
scale_angular_rate(raw[5]),
|
||||
],
|
||||
scale_temperature(bytes[13] as i8),
|
||||
),
|
||||
tick,
|
||||
))
|
||||
}
|
||||
|
||||
const fn read_i16(bytes: &[u8; SAMPLE_BYTES], offset: usize) -> i16 {
|
||||
i16::from_be_bytes([bytes[offset], bytes[offset + 1]])
|
||||
}
|
||||
|
||||
// The configured ±4 g range has 8,192 counts per g.
|
||||
// ±4 g: 8192 counts/g. Products fit i32; division truncates toward zero.
|
||||
const fn scale_acceleration(raw: i16) -> i16 {
|
||||
((raw as i32 * 1_000) / 8_192) as i16
|
||||
}
|
||||
|
||||
// The configured ±500°/s range has 65.5 counts/(°/s).
|
||||
// ±500°/s: 65.5 counts/(°/s).
|
||||
const fn scale_angular_rate(raw: i16) -> i32 {
|
||||
(raw as i32 * 17_453) / 65_500
|
||||
}
|
||||
|
||||
// ICM-42688-P temperature is 132.48 counts/°C with a 25°C offset.
|
||||
const fn scale_temperature(raw: i16) -> i16 {
|
||||
((raw as i32 * 10_000) / 13_248 + 2_500) as i16
|
||||
// FIFO temperature uses signed 8-bit data, 2.07 counts/°C, and a 25°C offset.
|
||||
const fn scale_temperature(raw: i8) -> i16 {
|
||||
((raw as i32 * 10_000) / 207 + 2_500) as i16
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{decode, scale_temperature};
|
||||
use kaptan_time::Timestamp;
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn decodes_the_icm_temperature_first_layout() {
|
||||
let bytes = [0, 0, 0x20, 0, 0, 0, 0, 0, 0x7f, 0xeb, 0, 0, 0, 0];
|
||||
let sample = decode(&bytes, Timestamp::from_microseconds(7));
|
||||
assert_eq!(sample.acceleration_milligravity[0], 1_000);
|
||||
assert_eq!(sample.angular_rate_milliradians_per_second[0], 8_725);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, 2_500);
|
||||
fn packet() -> [u8; SAMPLE_BYTES] {
|
||||
[
|
||||
0x68, 0x20, 0, 0, 0, 0, 0, 0x7f, 0xeb, 0, 0, 0, 0, 0, 0xff, 0xfe,
|
||||
]
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn signed_raw_endpoints_fit_the_configured_ranges() {
|
||||
for (temperature_raw, temperature) in [(i16::MIN, -22_234), (i16::MAX, 27_233)] {
|
||||
let mut bytes = [0; 14];
|
||||
for (index, raw) in [
|
||||
temperature_raw,
|
||||
i16::MIN,
|
||||
0,
|
||||
i16::MAX,
|
||||
i16::MIN,
|
||||
0,
|
||||
i16::MAX,
|
||||
]
|
||||
.into_iter()
|
||||
.enumerate()
|
||||
{
|
||||
bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes());
|
||||
}
|
||||
let timestamp = Timestamp::from_microseconds(123);
|
||||
let sample = decode(&bytes, timestamp);
|
||||
assert_eq!(sample.timestamp, timestamp);
|
||||
assert_eq!(sample.acceleration_milligravity, [-4_000, 0, 3_999]);
|
||||
fn packet_layout_and_signed_scales() {
|
||||
let timestamp = Timestamp::from_microseconds(7);
|
||||
let (sample, tick) = decode(&packet(), timestamp, None).unwrap();
|
||||
assert_eq!(sample.timestamp, timestamp);
|
||||
assert_eq!(sample.acceleration_milligravity, [1000, 0, 0]);
|
||||
assert_eq!(sample.angular_rate_milliradians_per_second, [8725, 0, 0]);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, 2500);
|
||||
assert_eq!(tick, 65534);
|
||||
let mut bytes = packet();
|
||||
for (offset, raw) in [(1, -32766i16), (5, 32767), (7, -32766), (11, 32767)] {
|
||||
bytes[offset..offset + 2].copy_from_slice(&raw.to_be_bytes());
|
||||
}
|
||||
let (sample, _) = decode(&bytes, timestamp, None).unwrap();
|
||||
assert_eq!(sample.acceleration_milligravity, [-3999, 0, 3999]);
|
||||
assert_eq!(
|
||||
sample.angular_rate_milliradians_per_second,
|
||||
[-8730, 0, 8731]
|
||||
);
|
||||
for raw in i8::MIN..=i8::MAX {
|
||||
let reference = f64::from(raw) / 2.07 * 100.0 + 2500.0;
|
||||
assert!((f64::from(scale_temperature(raw)) - reference).abs() < 1.0);
|
||||
}
|
||||
assert_eq!(scale_temperature(i8::MIN), -3683);
|
||||
assert_eq!(scale_temperature(i8::MAX), 8635);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn header_and_invalid_codes() {
|
||||
let mut bytes = packet();
|
||||
let now = Timestamp::from_microseconds(0);
|
||||
for header in 0..=u8::MAX {
|
||||
bytes[0] = header;
|
||||
assert_eq!(
|
||||
sample.angular_rate_milliradians_per_second,
|
||||
[-8_731, 0, 8_731]
|
||||
decode(&bytes, now, None).is_some(),
|
||||
(0x68..=0x6b).contains(&header)
|
||||
);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, temperature);
|
||||
assert_eq!(decode(&bytes, now, Some(65000)).is_some(), header == 0x68);
|
||||
}
|
||||
for offset in [1, 3, 5, 7, 9, 11] {
|
||||
for invalid in [i16::MIN, i16::MIN + 1] {
|
||||
bytes = packet();
|
||||
bytes[offset..offset + 2].copy_from_slice(&invalid.to_be_bytes());
|
||||
assert!(decode(&bytes, now, None).is_none());
|
||||
}
|
||||
}
|
||||
bytes = packet();
|
||||
bytes[13] = 0x80;
|
||||
assert!(decode(&bytes, now, None).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn temperature_matches_datasheet_scale_and_rounding() {
|
||||
// ±13,248 counts is ±100°C relative to the 25°C offset.
|
||||
for (raw, expected) in [
|
||||
(-13_248, -7_500),
|
||||
(-132, 2_401),
|
||||
(-1, 2_500),
|
||||
(0, 2_500),
|
||||
(1, 2_500),
|
||||
(132, 2_599),
|
||||
(13_248, 12_500),
|
||||
fn tick_order_allows_wrap() {
|
||||
let mut bytes = packet();
|
||||
let now = Timestamp::from_microseconds(0);
|
||||
for (previous, tick, valid) in [
|
||||
(65534, 936u16, true),
|
||||
(937, 1875, true),
|
||||
(937, 937, false),
|
||||
(937, 936, false),
|
||||
(937, 33705, false),
|
||||
] {
|
||||
assert_eq!(scale_temperature(raw), expected);
|
||||
}
|
||||
|
||||
// Independent floating-point datasheet reference over all 65,536 raw
|
||||
// codes. Truncation may lose less than one centidegree, never wrap.
|
||||
let mut previous = i16::MIN;
|
||||
for raw in i16::MIN..=i16::MAX {
|
||||
let actual = scale_temperature(raw);
|
||||
let reference = (f64::from(raw) / 132.48 + 25.0) * 100.0;
|
||||
assert!((f64::from(actual) - reference).abs() < 1.0);
|
||||
assert!(actual >= previous);
|
||||
previous = actual;
|
||||
bytes[14..].copy_from_slice(&tick.to_be_bytes());
|
||||
assert_eq!(decode(&bytes, now, Some(previous)).is_some(), valid);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,7 +1,7 @@
|
||||
//! Timestamped ICM-42688-P acquisition service.
|
||||
|
||||
use super::{Error, Icm42688, Icm42688Event};
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use crate::ImuBatch;
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use kaptan_time::MonotonicClock;
|
||||
|
||||
@@ -12,8 +12,8 @@ pub enum Icm42688ServiceEvent<BusError> {
|
||||
Idle,
|
||||
/// IMU configuration completed and the DRDY route may be enabled.
|
||||
Initialized,
|
||||
/// A coherent IMU observation is ready for publication or control.
|
||||
Sample(ImuSnapshot),
|
||||
/// A checked FIFO batch is ready for publication.
|
||||
Samples(ImuBatch),
|
||||
/// The ICM-42688-P path entered a terminal fault state.
|
||||
Fault(Error<BusError>),
|
||||
}
|
||||
@@ -46,13 +46,13 @@ where
|
||||
service_event(self.imu.on_periodic_tick(self.clock.now()))
|
||||
}
|
||||
|
||||
/// Handles one coalesced data-ready notification. Time is captured here in
|
||||
/// the worker, at read submission, not at IRQ entry or physical sampling.
|
||||
/// Starts a drain from one coalesced notification. Packet timestamps are
|
||||
/// captured at their read submissions, not at IRQ entry or sampling.
|
||||
pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
|
||||
if !self.imu.is_ready() {
|
||||
return Icm42688ServiceEvent::Idle;
|
||||
}
|
||||
match self.imu.request_sample(self.clock.now()) {
|
||||
match self.imu.request_samples(self.clock.now()) {
|
||||
Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle,
|
||||
Err(error) => Icm42688ServiceEvent::Fault(error),
|
||||
}
|
||||
@@ -69,9 +69,9 @@ where
|
||||
|
||||
fn service_event<E>(event: Icm42688Event<E>) -> Icm42688ServiceEvent<E> {
|
||||
match event {
|
||||
Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle,
|
||||
Icm42688Event::NotPending | Icm42688Event::InProgress => Icm42688ServiceEvent::Idle,
|
||||
Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized,
|
||||
Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
|
||||
Icm42688Event::Samples(batch) => Icm42688ServiceEvent::Samples(batch),
|
||||
Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error),
|
||||
}
|
||||
}
|
||||
@@ -79,7 +79,7 @@ fn service_event<E>(event: Icm42688Event<E>) -> Icm42688ServiceEvent<E> {
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::mpu6000::test_bus::FakeBus;
|
||||
use crate::icm42688::tests::{initialized, packet};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
struct Clock(u64);
|
||||
@@ -89,76 +89,62 @@ mod tests {
|
||||
}
|
||||
}
|
||||
|
||||
fn initialized() -> Icm42688Service<Clock, FakeBus> {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.response[1] = 0x47;
|
||||
let mut service = Icm42688Service::new(Clock(0), Icm42688::new(bus));
|
||||
service.begin().unwrap();
|
||||
service.clock.0 = 999;
|
||||
service.on_periodic_tick();
|
||||
assert_eq!(service.imu.bus.starts, 0);
|
||||
service.clock.0 = 1_000;
|
||||
service.on_periodic_tick();
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.clock.0 = 1_999;
|
||||
service.on_periodic_tick();
|
||||
assert_eq!(service.imu.bus.starts, 1);
|
||||
service.clock.0 = 2_000;
|
||||
service.on_periodic_tick();
|
||||
for _ in 0..8 {
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
}
|
||||
service.clock.0 = 46_999;
|
||||
service.on_periodic_tick();
|
||||
assert_eq!(service.imu.bus.starts, 9);
|
||||
service.clock.0 = 47_000;
|
||||
service.on_periodic_tick();
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.clock.0 = 48_000;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Icm42688ServiceEvent::Initialized
|
||||
));
|
||||
service
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn fake_clock_labels_the_accepted_read_request_even_after_busy_admission() {
|
||||
let mut service = initialized();
|
||||
service.clock.0 = 48_100;
|
||||
service.imu.bus.busy = true;
|
||||
fn clock_labels_each_accepted_packet_read() {
|
||||
let mut service = Icm42688Service::new(Clock(50_000), initialized());
|
||||
service.imu.bus.response.fill(0);
|
||||
assert!(matches!(
|
||||
service.on_data_ready(),
|
||||
Icm42688ServiceEvent::Idle
|
||||
));
|
||||
service.clock.0 = 48_200;
|
||||
service.imu.bus.response[1..3].copy_from_slice(&16u16.to_be_bytes());
|
||||
service.clock.0 = 50_100;
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.imu.bus.busy = true;
|
||||
service.clock.0 = 50_200;
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.imu.bus.busy = false;
|
||||
service.imu.bus.response.fill(0);
|
||||
service.imu.bus.response[1..17].copy_from_slice(&packet(937, 0));
|
||||
service.clock.0 = 50_300;
|
||||
service.on_periodic_tick();
|
||||
service.clock.0 = 48_500;
|
||||
assert!(
|
||||
matches!(service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Icm42688ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(48_200))
|
||||
service.imu.bus.response.fill(0);
|
||||
for now in [50_400, 50_500] {
|
||||
service.clock.0 = now;
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Icm42688ServiceEvent::Idle
|
||||
));
|
||||
}
|
||||
service.clock.0 = 50_600;
|
||||
let Icm42688ServiceEvent::Samples(batch) =
|
||||
service.on_transfer_complete(SpiCompletion::Complete)
|
||||
else {
|
||||
panic!("missing checked batch");
|
||||
};
|
||||
assert_eq!(
|
||||
batch.as_slice()[0].timestamp,
|
||||
Timestamp::from_microseconds(50_300)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn fake_clock_reports_missing_drdy_and_dma_without_polling_samples() {
|
||||
let mut service = initialized();
|
||||
service.clock.0 = 59_999;
|
||||
fn missing_drdy_and_dma_are_terminal() {
|
||||
let mut service = Icm42688Service::new(Clock(61_699), initialized());
|
||||
let starts = service.imu.bus.starts;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Icm42688ServiceEvent::Idle
|
||||
));
|
||||
assert_eq!(service.imu.bus.starts, 11);
|
||||
service.clock.0 = 60_000;
|
||||
assert_eq!(service.imu.bus.starts, starts);
|
||||
service.clock.0 = 61_700;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Icm42688ServiceEvent::Fault(Error::DataReadyTimeout)
|
||||
));
|
||||
let mut service = initialized();
|
||||
let mut service = Icm42688Service::new(Clock(50_000), initialized());
|
||||
service.on_data_ready();
|
||||
service.clock.0 = 58_000;
|
||||
service.clock.0 = 60_000;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Icm42688ServiceEvent::Fault(Error::TransferTimeout)
|
||||
|
||||
@@ -11,3 +11,57 @@ pub mod mpu6000;
|
||||
pub mod lidar_lite_v3;
|
||||
/// MicoAir MTF-01 Microlink UART range and optical-flow driver.
|
||||
pub mod mtf01;
|
||||
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
|
||||
/// Nonempty FIFO drain, published only after the driver checks for overflow.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct ImuBatch {
|
||||
samples: [ImuSnapshot; Self::CAPACITY],
|
||||
len: u8,
|
||||
}
|
||||
|
||||
impl ImuBatch {
|
||||
/// Maximum records reserved by one driver drain.
|
||||
pub const CAPACITY: usize = 4;
|
||||
|
||||
pub const fn new(first: ImuSnapshot) -> Self {
|
||||
Self {
|
||||
samples: [first; Self::CAPACITY],
|
||||
len: 1,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn push(&mut self, sample: ImuSnapshot) -> Result<(), ImuSnapshot> {
|
||||
let Some(slot) = self.samples.get_mut(usize::from(self.len)) else {
|
||||
return Err(sample);
|
||||
};
|
||||
*slot = sample;
|
||||
self.len += 1;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn as_slice(&self) -> &[ImuSnapshot] {
|
||||
&self.samples[..usize::from(self.len)]
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
#[test]
|
||||
fn full_batch_preserves_accepted_samples() {
|
||||
let sample = |time| ImuSnapshot::new(Timestamp::from_microseconds(time), [0; 3], [0; 3], 0);
|
||||
let mut batch = ImuBatch::new(sample(0));
|
||||
for time in 1..4 {
|
||||
batch.push(sample(time)).unwrap();
|
||||
}
|
||||
assert_eq!(batch.push(sample(4)), Err(sample(4)));
|
||||
assert_eq!(
|
||||
batch.as_slice(),
|
||||
&[sample(0), sample(1), sample(2), sample(3)]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -27,7 +27,16 @@ pub enum Error<BusError> {
|
||||
/// The bus stayed busy for the pending operation's 20 ms admission budget.
|
||||
#[kaptan_error(id = 5007)]
|
||||
AdmissionTimeout,
|
||||
/// No acquisition was requested within two output periods plus 10 ms.
|
||||
/// No checked, nonempty batch arrived within two output periods plus 10 ms.
|
||||
#[kaptan_error(id = 5008)]
|
||||
DataReadyTimeout,
|
||||
/// FIFO overflow destroyed the ordering of unread sensor records.
|
||||
#[kaptan_error(id = 5009)]
|
||||
FifoOverflow,
|
||||
/// The reported FIFO count exceeds its physical capacity.
|
||||
#[kaptan_error(id = 5010)]
|
||||
FifoCount(u16),
|
||||
/// FIFO control does not match the required reset or acquisition state.
|
||||
#[kaptan_error(id = 5011)]
|
||||
FifoControl(u8),
|
||||
}
|
||||
@@ -9,7 +9,7 @@ pub mod service;
|
||||
#[cfg(test)]
|
||||
pub(crate) mod test_bus;
|
||||
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use crate::ImuBatch;
|
||||
use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
@@ -18,10 +18,11 @@ pub use error::Error;
|
||||
pub use service::{Mpu6000Service, Mpu6000ServiceEvent};
|
||||
|
||||
use registers::{
|
||||
ACCEL_CONFIG, ACCEL_XOUT_H, CLOCK_SOURCE_PLL_X_GYRO, CONFIG, DATA_READY_INTERRUPT,
|
||||
DEVICE_RESET, ENABLE_ALL_SENSOR_AXES, EXPECTED_WHO_AM_I, GYRO_CONFIG, I2C_INTERFACE_DISABLE,
|
||||
INT_ENABLE, INT_PIN_CFG, INTERRUPT_CLEAR_ON_ANY_READ, PWR_MGMT_1, PWR_MGMT_2, SAMPLE_BYTES,
|
||||
SMPLRT_DIV, USER_CTRL, WHO_AM_I,
|
||||
ACCEL_CONFIG, CLOCK_SOURCE_PLL_X_GYRO, CONFIG, DATA_READY_INTERRUPT, DEVICE_RESET,
|
||||
ENABLE_ALL_SENSOR_AXES, EXPECTED_WHO_AM_I, FIFO_CAPACITY, FIFO_COUNT_H, FIFO_EN, FIFO_ENABLE,
|
||||
FIFO_OVERFLOW_INTERRUPT, FIFO_R_W, FIFO_RESET, FIFO_SENSOR_CHANNELS, GYRO_CONFIG,
|
||||
I2C_INTERFACE_DISABLE, INT_ENABLE, INT_PIN_CFG, INT_STATUS, PWR_MGMT_1, PWR_MGMT_2,
|
||||
SAMPLE_BYTES, SMPLRT_DIV, USER_CTRL, WHO_AM_I,
|
||||
};
|
||||
|
||||
const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
|
||||
@@ -40,7 +41,7 @@ const WAKE_SETTLE_US: u64 = 100_000;
|
||||
const TRANSFER_TIMEOUT_US: u64 = 10_000;
|
||||
// Allows a retry on the F722's 10 ms periodic service before admission expires.
|
||||
const ADMISSION_TIMEOUT_US: u64 = 20_000;
|
||||
const CONFIGURATION: [(u8, u8); 9] = [
|
||||
const CONFIGURATION: [(u8, u8); 8] = [
|
||||
(PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO),
|
||||
(USER_CTRL, I2C_INTERFACE_DISABLE),
|
||||
(PWR_MGMT_2, ENABLE_ALL_SENSOR_AXES),
|
||||
@@ -48,8 +49,13 @@ const CONFIGURATION: [(u8, u8); 9] = [
|
||||
(SMPLRT_DIV, 0),
|
||||
(ACCEL_CONFIG, 0),
|
||||
(GYRO_CONFIG, 0),
|
||||
(INT_PIN_CFG, INTERRUPT_CLEAR_ON_ANY_READ),
|
||||
(INT_ENABLE, DATA_READY_INTERRUPT),
|
||||
// Only INT_STATUS reads clear overflow, including throughout FIFO drains.
|
||||
(INT_PIN_CFG, 0),
|
||||
];
|
||||
const FIFO_CONFIGURATION: [(u8, u8); 3] = [
|
||||
(FIFO_EN, FIFO_SENSOR_CHANNELS),
|
||||
(USER_CTRL, I2C_INTERFACE_DISABLE | FIFO_ENABLE),
|
||||
(INT_ENABLE, DATA_READY_INTERRUPT | FIFO_OVERFLOW_INTERRUPT),
|
||||
];
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
@@ -71,9 +77,7 @@ enum State {
|
||||
Settling {
|
||||
since: Timestamp,
|
||||
},
|
||||
Ready {
|
||||
since: Timestamp,
|
||||
},
|
||||
Ready,
|
||||
Faulted,
|
||||
}
|
||||
|
||||
@@ -83,7 +87,25 @@ enum Operation {
|
||||
ResetSignalPaths,
|
||||
Identity,
|
||||
Configure(usize),
|
||||
Sample,
|
||||
ResetFifo,
|
||||
CheckFifoReset,
|
||||
ConfigureFifo(usize),
|
||||
StartFifo,
|
||||
FifoStatus,
|
||||
FifoCount,
|
||||
FifoSample(u8),
|
||||
FifoCheck,
|
||||
FifoControl,
|
||||
}
|
||||
|
||||
impl Operation {
|
||||
fn transfer_length(self) -> usize {
|
||||
match self {
|
||||
Self::FifoCount => 3,
|
||||
Self::FifoSample(_) => TRANSFER_CAPACITY,
|
||||
_ => 2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Observable outcome of one timer or SPI terminal event.
|
||||
@@ -91,12 +113,12 @@ enum Operation {
|
||||
pub enum Mpu6000Event<BusError> {
|
||||
/// No sample, initialization completion or terminal fault was produced.
|
||||
NotPending,
|
||||
/// One initialization transfer completed; initialization continues.
|
||||
Initializing,
|
||||
/// One transfer completed; initialization or acquisition continues.
|
||||
InProgress,
|
||||
/// The MPU6000 identity and configuration sequence completed.
|
||||
Initialized,
|
||||
/// One coherent IMU sample completed.
|
||||
Sample(ImuSnapshot),
|
||||
/// A bounded, overflow-checked FIFO drain completed.
|
||||
Samples(ImuBatch),
|
||||
/// Initialization or acquisition failed.
|
||||
Fault(Error<BusError>),
|
||||
}
|
||||
@@ -109,6 +131,8 @@ pub struct Mpu6000<Bus> {
|
||||
bus: Bus,
|
||||
config: Mpu6000Config,
|
||||
state: State,
|
||||
progress_at: Option<Timestamp>,
|
||||
batch: Option<ImuBatch>,
|
||||
transmit: [u8; TRANSFER_CAPACITY],
|
||||
receive: [u8; TRANSFER_CAPACITY],
|
||||
}
|
||||
@@ -120,6 +144,8 @@ impl<Bus> Mpu6000<Bus> {
|
||||
bus,
|
||||
config,
|
||||
state: State::Dormant,
|
||||
progress_at: None,
|
||||
batch: None,
|
||||
transmit: [0; TRANSFER_CAPACITY],
|
||||
receive: [0; TRANSFER_CAPACITY],
|
||||
}
|
||||
@@ -154,8 +180,7 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
self.queue(next, now);
|
||||
}
|
||||
State::Settling { since } if elapsed(now, since, self.sample_period_us()) => {
|
||||
self.state = State::Ready { since: now };
|
||||
return Mpu6000Event::Initialized;
|
||||
self.queue(Operation::ResetFifo, now);
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
@@ -165,17 +190,16 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
}
|
||||
}
|
||||
|
||||
/// Accepts a burst-read request. Busy admission remains pending for periodic
|
||||
/// retry. The eventual accepted submission time becomes the snapshot
|
||||
/// timestamp, not the ISR time or a physical sampling epoch.
|
||||
pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
/// Accepts one bounded FIFO drain. Each snapshot records its packet-read
|
||||
/// submission time, not the ISR time or a physical sampling epoch.
|
||||
pub fn request_samples(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
if let Some(error) = self.check_deadline(now) {
|
||||
return Err(error);
|
||||
}
|
||||
if !self.is_ready() {
|
||||
return Err(Error::NotReady);
|
||||
}
|
||||
self.queue(Operation::Sample, now);
|
||||
self.queue(Operation::FifoStatus, now);
|
||||
self.submit_pending(now)
|
||||
}
|
||||
|
||||
@@ -194,31 +218,21 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
let State::Active { operation, since } = self.state else {
|
||||
return Mpu6000Event::NotPending;
|
||||
};
|
||||
let length = if operation == Operation::Sample {
|
||||
TRANSFER_CAPACITY
|
||||
} else {
|
||||
2
|
||||
};
|
||||
let length = operation.transfer_length();
|
||||
match self
|
||||
.bus
|
||||
.complete_transfer(completion, &mut self.receive[..length])
|
||||
{
|
||||
Ok(()) => self.advance_after_completion(operation, since, now),
|
||||
Err(CompletionError::NotPending) => Mpu6000Event::NotPending,
|
||||
Err(CompletionError::TransferFault) => {
|
||||
self.state = State::Faulted;
|
||||
Mpu6000Event::Fault(Error::TransferFault)
|
||||
}
|
||||
Err(error) => {
|
||||
self.state = State::Faulted;
|
||||
Mpu6000Event::Fault(Error::Completion(error))
|
||||
}
|
||||
Err(CompletionError::TransferFault) => self.fault(Error::TransferFault),
|
||||
Err(error) => self.fault(Error::Completion(error)),
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns whether an IMU data-ready event can start a sensor transfer.
|
||||
pub fn is_ready(&self) -> bool {
|
||||
matches!(self.state, State::Ready { .. })
|
||||
self.state == State::Ready
|
||||
}
|
||||
|
||||
fn advance_after_completion(
|
||||
@@ -233,8 +247,7 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
Operation::Identity => {
|
||||
let identity = self.receive[1];
|
||||
if identity != EXPECTED_WHO_AM_I {
|
||||
self.state = State::Faulted;
|
||||
return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity));
|
||||
return self.fault(Error::UnexpectedIdentity(identity));
|
||||
}
|
||||
|
||||
self.queue(Operation::Configure(0), now);
|
||||
@@ -244,22 +257,93 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
if index == 0 {
|
||||
self.wait(Operation::Configure(next), now, WAKE_SETTLE_US);
|
||||
} else if next == CONFIGURATION.len() {
|
||||
// Allow a complete output period after the last write before
|
||||
// accepting DRDY. RM §4.2 defines the configured sample rate.
|
||||
// FIFO stays disabled until the configured output has settled.
|
||||
self.state = State::Settling { since: now };
|
||||
} else {
|
||||
self.queue(Operation::Configure(next), now);
|
||||
}
|
||||
}
|
||||
Operation::Sample => {
|
||||
Operation::ResetFifo => self.queue(Operation::CheckFifoReset, now),
|
||||
Operation::CheckFifoReset => {
|
||||
if self.receive[1] != I2C_INTERFACE_DISABLE {
|
||||
return self.fault(Error::FifoControl(self.receive[1]));
|
||||
}
|
||||
self.queue(Operation::ConfigureFifo(0), now);
|
||||
}
|
||||
Operation::ConfigureFifo(index) => {
|
||||
self.queue(
|
||||
if index + 1 == FIFO_CONFIGURATION.len() {
|
||||
Operation::StartFifo
|
||||
} else {
|
||||
Operation::ConfigureFifo(index + 1)
|
||||
},
|
||||
now,
|
||||
);
|
||||
}
|
||||
Operation::StartFifo | Operation::FifoStatus | Operation::FifoCheck => {
|
||||
if self.receive[1] & FIFO_OVERFLOW_INTERRUPT != 0 {
|
||||
return self.fault(Error::FifoOverflow);
|
||||
}
|
||||
match operation {
|
||||
Operation::StartFifo => {
|
||||
self.state = State::Ready;
|
||||
self.progress_at = Some(now);
|
||||
return Mpu6000Event::Initialized;
|
||||
}
|
||||
Operation::FifoCheck => {
|
||||
self.queue(Operation::FifoControl, now);
|
||||
}
|
||||
_ => self.queue(Operation::FifoCount, now),
|
||||
}
|
||||
}
|
||||
Operation::FifoCount => {
|
||||
let count = u16::from_be_bytes([self.receive[1], self.receive[2]]);
|
||||
if usize::from(count) > FIFO_CAPACITY {
|
||||
return self.fault(Error::FifoCount(count));
|
||||
}
|
||||
// The count is latched high-byte first. A trailing partial
|
||||
// record may still be arriving; leave it in the FIFO.
|
||||
let packets = (usize::from(count) / SAMPLE_BYTES).min(ImuBatch::CAPACITY);
|
||||
if packets == 0 {
|
||||
self.state = State::Ready;
|
||||
return Mpu6000Event::NotPending;
|
||||
}
|
||||
self.queue(Operation::FifoSample(packets as u8), now);
|
||||
}
|
||||
Operation::FifoSample(remaining) => {
|
||||
let mut bytes = [0; SAMPLE_BYTES];
|
||||
bytes.copy_from_slice(&self.receive[1..]);
|
||||
self.state = State::Ready { since: now };
|
||||
return Mpu6000Event::Sample(sample::decode(&bytes, requested, self.config));
|
||||
let sample = sample::decode(&bytes, requested, self.config);
|
||||
match &mut self.batch {
|
||||
Some(batch) => batch
|
||||
.push(sample)
|
||||
.expect("count is bounded by batch capacity"),
|
||||
None => self.batch = Some(ImuBatch::new(sample)),
|
||||
}
|
||||
self.queue(
|
||||
if remaining == 1 {
|
||||
Operation::FifoCheck
|
||||
} else {
|
||||
Operation::FifoSample(remaining - 1)
|
||||
},
|
||||
now,
|
||||
);
|
||||
}
|
||||
Operation::FifoControl => {
|
||||
// A reset can disable FIFO after the count read. Empty FIFO
|
||||
// reads repeat its last byte, and reset INT_STATUS is zero.
|
||||
if self.receive[1] != (I2C_INTERFACE_DISABLE | FIFO_ENABLE) {
|
||||
return self.fault(Error::FifoControl(self.receive[1]));
|
||||
}
|
||||
self.state = State::Ready;
|
||||
self.progress_at = Some(now);
|
||||
return Mpu6000Event::Samples(
|
||||
self.batch.take().expect("checked drain contains a packet"),
|
||||
);
|
||||
}
|
||||
}
|
||||
match self.submit_pending(now) {
|
||||
Ok(()) => Mpu6000Event::Initializing,
|
||||
Ok(()) => Mpu6000Event::InProgress,
|
||||
Err(error) => Mpu6000Event::Fault(error),
|
||||
}
|
||||
}
|
||||
@@ -276,11 +360,10 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
self.prepare_write(register, value);
|
||||
}
|
||||
|
||||
fn prepare_read(&mut self, register: u8, length: usize) {
|
||||
fn prepare_read(&mut self, register: u8) {
|
||||
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) {
|
||||
@@ -311,15 +394,23 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
match operation {
|
||||
Operation::Reset => self.prepare_write(PWR_MGMT_1, DEVICE_RESET),
|
||||
Operation::ResetSignalPaths => self.prepare_write(SIGNAL_PATH_RESET, 0x07),
|
||||
Operation::Identity => self.prepare_read(WHO_AM_I, 2),
|
||||
Operation::Identity => self.prepare_read(WHO_AM_I),
|
||||
Operation::Configure(index) => self.prepare_configuration(index),
|
||||
Operation::Sample => self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY),
|
||||
Operation::ResetFifo => {
|
||||
self.prepare_write(USER_CTRL, I2C_INTERFACE_DISABLE | FIFO_RESET)
|
||||
}
|
||||
Operation::CheckFifoReset | Operation::FifoControl => self.prepare_read(USER_CTRL),
|
||||
Operation::ConfigureFifo(index) => {
|
||||
let (register, value) = FIFO_CONFIGURATION[index];
|
||||
self.prepare_write(register, value);
|
||||
}
|
||||
Operation::StartFifo | Operation::FifoStatus | Operation::FifoCheck => {
|
||||
self.prepare_read(INT_STATUS);
|
||||
}
|
||||
Operation::FifoCount => self.prepare_read(FIFO_COUNT_H),
|
||||
Operation::FifoSample(_) => self.prepare_read(FIFO_R_W),
|
||||
}
|
||||
let length = if operation == Operation::Sample {
|
||||
TRANSFER_CAPACITY
|
||||
} else {
|
||||
2
|
||||
};
|
||||
let length = operation.transfer_length();
|
||||
match self.bus.start_transfer(&self.transmit[..length]) {
|
||||
Ok(()) => {
|
||||
self.state = State::Active {
|
||||
@@ -330,7 +421,7 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
}
|
||||
Err(error) if Bus::is_busy(&error) => Ok(()),
|
||||
Err(error) => {
|
||||
self.state = State::Faulted;
|
||||
self.stop();
|
||||
Err(Error::Start(error))
|
||||
}
|
||||
}
|
||||
@@ -348,35 +439,50 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
fn check_deadline(&mut self, now: Timestamp) -> Option<Error<Bus::StartError>> {
|
||||
let error = match self.state {
|
||||
State::Active { since, .. } if elapsed(now, since, TRANSFER_TIMEOUT_US) => {
|
||||
// Terminal fault completion is also the bus cancellation contract.
|
||||
// It proves quiescence or quarantines permanent DMA storage.
|
||||
match self
|
||||
.bus
|
||||
.complete_transfer(SpiCompletion::TransferFault, &mut [])
|
||||
{
|
||||
Err(CompletionError::TransferFault | CompletionError::NotActive) | Ok(()) => {
|
||||
Error::TransferTimeout
|
||||
}
|
||||
Err(error) => Error::Completion(error),
|
||||
}
|
||||
Error::TransferTimeout
|
||||
}
|
||||
State::Pending { since, .. } if elapsed(now, since, ADMISSION_TIMEOUT_US) => {
|
||||
Error::AdmissionTimeout
|
||||
}
|
||||
State::Ready { since }
|
||||
if elapsed(
|
||||
_ if self.progress_at.is_some_and(|since| {
|
||||
elapsed(
|
||||
now,
|
||||
since,
|
||||
2 * self.sample_period_us() + TRANSFER_TIMEOUT_US,
|
||||
) =>
|
||||
)
|
||||
}) =>
|
||||
{
|
||||
Error::DataReadyTimeout
|
||||
}
|
||||
_ => return None,
|
||||
};
|
||||
self.state = State::Faulted;
|
||||
// Every active timeout uses the bus cancellation contract, which proves
|
||||
// quiescence or quarantines permanent DMA storage.
|
||||
let error = if matches!(self.state, State::Active { .. }) {
|
||||
match self
|
||||
.bus
|
||||
.complete_transfer(SpiCompletion::TransferFault, &mut [])
|
||||
{
|
||||
Err(CompletionError::TransferFault | CompletionError::NotActive) | Ok(()) => error,
|
||||
Err(error) => Error::Completion(error),
|
||||
}
|
||||
} else {
|
||||
error
|
||||
};
|
||||
self.stop();
|
||||
Some(error)
|
||||
}
|
||||
|
||||
fn stop(&mut self) {
|
||||
self.state = State::Faulted;
|
||||
self.batch = None;
|
||||
self.progress_at = None;
|
||||
}
|
||||
|
||||
fn fault(&mut self, error: Error<Bus::StartError>) -> Mpu6000Event<Bus::StartError> {
|
||||
self.stop();
|
||||
Mpu6000Event::Fault(error)
|
||||
}
|
||||
}
|
||||
|
||||
fn elapsed(now: Timestamp, since: Timestamp, duration_us: u64) -> bool {
|
||||
@@ -390,154 +496,278 @@ mod tests {
|
||||
use super::test_bus::{FakeBus, FakeError};
|
||||
use super::*;
|
||||
|
||||
const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
pub(super) const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||
gyroscope: GyroscopeRange::PlusMinus1000DegreesPerSecond,
|
||||
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
|
||||
sample_rate_divider: 9,
|
||||
};
|
||||
const BURST: [u8; 14] = [
|
||||
0x20, 0, 0xe0, 0, 0, 0, // 1g, -1g, 0g
|
||||
0, 0, // 36.53 C
|
||||
0x40, 0x10, 0xbf, 0xf0, 0, 0, // 500, -500, 0 degrees/s
|
||||
];
|
||||
|
||||
fn t(us: u64) -> Timestamp {
|
||||
Timestamp::from_microseconds(us + 100_000)
|
||||
Timestamp::from_microseconds(us)
|
||||
}
|
||||
|
||||
fn start(driver: &mut Mpu6000<FakeBus>) {
|
||||
driver
|
||||
.begin_initialization(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.on_periodic_tick(Timestamp::from_microseconds(99_999));
|
||||
assert_eq!(driver.bus.starts, 0);
|
||||
driver.on_periodic_tick(t(0));
|
||||
fn response(driver: &mut Mpu6000<FakeBus>, bytes: &[u8]) {
|
||||
driver.bus.response.fill(0);
|
||||
driver.bus.response[1..=bytes.len()].copy_from_slice(bytes);
|
||||
}
|
||||
|
||||
fn initialized() -> Mpu6000<FakeBus> {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.response[1] = 0x68;
|
||||
let mut driver = Mpu6000::new(bus, TEST_CONFIG);
|
||||
start(&mut driver);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100));
|
||||
driver.on_periodic_tick(t(100_100));
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0x68, 7]);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
|
||||
driver.on_periodic_tick(t(200_200));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
|
||||
driver.on_periodic_tick(t(300_400));
|
||||
for index in 1..9 {
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
|
||||
fn complete(driver: &mut Mpu6000<FakeBus>, now: u64) -> Mpu6000Event<FakeError> {
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(now))
|
||||
}
|
||||
|
||||
pub(super) fn initialized() -> Mpu6000<FakeBus> {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.begin_initialization(t(0)).unwrap();
|
||||
for (at, command) in [(100_000, [0x6b, 0x80]), (200_000, [0x68, 7])] {
|
||||
driver.on_periodic_tick(t(at - 1));
|
||||
assert!(!driver.is_ready());
|
||||
driver.on_periodic_tick(t(at));
|
||||
assert_eq!(&driver.bus.transmitted[..2], &command);
|
||||
complete(&mut driver, at);
|
||||
}
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(311_200)),
|
||||
Mpu6000Event::Initialized
|
||||
);
|
||||
assert!(driver.is_ready());
|
||||
response(&mut driver, &[0x68]);
|
||||
driver.on_periodic_tick(t(300_000));
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0xf5, 0]);
|
||||
response(&mut driver, &[0]);
|
||||
complete(&mut driver, 300_000);
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0x6b, 1]);
|
||||
complete(&mut driver, 300_000);
|
||||
driver.on_periodic_tick(t(399_999));
|
||||
assert_eq!(driver.bus.starts, 4);
|
||||
driver.on_periodic_tick(t(400_000));
|
||||
for command in [
|
||||
[0x6a, 0x10],
|
||||
[0x6c, 0],
|
||||
[0x1a, 3],
|
||||
[0x19, 9],
|
||||
[0x1c, 8],
|
||||
[0x1b, 0x10],
|
||||
[0x37, 0],
|
||||
] {
|
||||
assert_eq!(&driver.bus.transmitted[..2], &command);
|
||||
complete(&mut driver, 400_000);
|
||||
}
|
||||
driver.on_periodic_tick(t(409_999));
|
||||
assert_eq!(driver.bus.starts, 11);
|
||||
assert_eq!(driver.request_samples(t(409_999)), Err(Error::NotReady));
|
||||
driver.on_periodic_tick(t(410_000));
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0x6a, 0x14]);
|
||||
response(&mut driver, &[0x10]);
|
||||
complete(&mut driver, 410_000);
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0xea, 0]);
|
||||
response(&mut driver, &[0]);
|
||||
complete(&mut driver, 410_000);
|
||||
for command in [[0x23, 0xf8], [0x6a, 0x50], [0x38, 0x11]] {
|
||||
assert_eq!(&driver.bus.transmitted[..2], &command);
|
||||
complete(&mut driver, 410_000);
|
||||
}
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0xba, 0]);
|
||||
assert_eq!(complete(&mut driver, 410_000), Mpu6000Event::Initialized);
|
||||
driver
|
||||
}
|
||||
|
||||
fn begin_drain(driver: &mut Mpu6000<FakeBus>, count: u16, now: u64) -> Mpu6000Event<FakeError> {
|
||||
response(driver, &[0]);
|
||||
driver.request_samples(t(now)).unwrap();
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0xba, 0]);
|
||||
response(driver, &count.to_be_bytes());
|
||||
complete(driver, now);
|
||||
assert_eq!(&driver.bus.transmitted[..3], &[0xf2, 0, 0]);
|
||||
response(driver, &BURST);
|
||||
complete(driver, now)
|
||||
}
|
||||
|
||||
fn operations() -> impl Iterator<Item = Operation> {
|
||||
[
|
||||
Operation::Reset,
|
||||
Operation::ResetSignalPaths,
|
||||
Operation::Identity,
|
||||
Operation::ResetFifo,
|
||||
Operation::CheckFifoReset,
|
||||
Operation::StartFifo,
|
||||
Operation::FifoStatus,
|
||||
Operation::FifoCount,
|
||||
Operation::FifoSample(1),
|
||||
Operation::FifoCheck,
|
||||
Operation::FifoControl,
|
||||
]
|
||||
.into_iter()
|
||||
.chain((0..CONFIGURATION.len()).map(Operation::Configure))
|
||||
.chain((0..FIFO_CONFIGURATION.len()).map(Operation::ConfigureFifo))
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_signal_reset_and_wake_wait_elapsed_time_from_completion() {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.bus.response[1] = 0x68;
|
||||
start(&mut driver);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100));
|
||||
for now in [100, 100, 50_000, 100_099] {
|
||||
driver.on_periodic_tick(t(now));
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
fn startup_keeps_fifo_disabled_until_sensor_settles() {
|
||||
let driver = initialized();
|
||||
assert!(driver.is_ready());
|
||||
assert_eq!(driver.batch, None);
|
||||
assert_eq!(driver.progress_at, Some(t(410_000)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn invalid_identity_or_fifo_reset_faults() {
|
||||
for (operation, value, error) in [
|
||||
(Operation::Identity, 0x70, Error::UnexpectedIdentity(0x70)),
|
||||
(Operation::CheckFifoReset, 0x14, Error::FifoControl(0x14)),
|
||||
(Operation::CheckFifoReset, 0x50, Error::FifoControl(0x50)),
|
||||
] {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
response(&mut driver, &[value]);
|
||||
driver.queue(operation, t(0));
|
||||
driver.submit_pending(t(0)).unwrap();
|
||||
assert_eq!(complete(&mut driver, 1), Mpu6000Event::Fault(error));
|
||||
assert!(!driver.is_ready());
|
||||
}
|
||||
driver.on_periodic_tick(t(100_100));
|
||||
assert_eq!(driver.bus.starts, 2);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
|
||||
driver.on_periodic_tick(t(200_199));
|
||||
assert_eq!(driver.bus.starts, 2);
|
||||
driver.on_periodic_tick(t(200_200));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
|
||||
driver.on_periodic_tick(t(300_399));
|
||||
assert_eq!(driver.bus.starts, 4);
|
||||
driver.on_periodic_tick(t(300_400));
|
||||
for index in 1..9 {
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
|
||||
}
|
||||
driver.on_periodic_tick(t(311_199));
|
||||
assert!(!driver.is_ready());
|
||||
assert_eq!(driver.request_sample(t(311_199)), Err(Error::NotReady));
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(311_200)),
|
||||
Mpu6000Event::Initialized
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_an_unexpected_identity() {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.response[1] = 0x70;
|
||||
let mut driver = Mpu6000::new(bus, TEST_CONFIG);
|
||||
|
||||
driver.queue(Operation::Identity, t(0));
|
||||
driver.submit_pending(t(0)).unwrap();
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100)),
|
||||
Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70))
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn converts_a_complete_burst_to_core_units() {
|
||||
let burst = [
|
||||
0x20, 0x00, 0xE0, 0x00, 0x00, 0x00, // accel: 1g, -1g, 0g
|
||||
0x00, 0x00, // 36.53 C
|
||||
0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps
|
||||
];
|
||||
fn packet_bytes_remain_owned_until_checked_publication() {
|
||||
let mut driver = initialized();
|
||||
driver.bus.response[1..].copy_from_slice(&burst);
|
||||
driver.request_sample(t(312_000)).unwrap();
|
||||
assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
|
||||
begin_drain(&mut driver, 14, 411_000);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::NotPending, t(312_100)),
|
||||
&driver.bus.transmitted[..15],
|
||||
&[0xf4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::NotPending, t(411_010)),
|
||||
Mpu6000Event::NotPending
|
||||
);
|
||||
assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
|
||||
// Overwrite staging bytes and move the whole driver while work is pending.
|
||||
// The fake bus, like the DMA route, owns the pending response itself.
|
||||
driver.transmit.fill(0xff);
|
||||
driver.bus.response.fill(0xff);
|
||||
let mut driver =
|
||||
core::mem::replace(&mut driver, Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
|
||||
let Mpu6000Event::Sample(sample) =
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(312_200))
|
||||
else {
|
||||
panic!("sample transfer must complete");
|
||||
response(&mut driver, &[0]);
|
||||
assert_eq!(complete(&mut driver, 411_100), Mpu6000Event::InProgress);
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0xba, 0]);
|
||||
response(&mut driver, &[0x50]);
|
||||
assert_eq!(complete(&mut driver, 411_200), Mpu6000Event::InProgress);
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0xea, 0]);
|
||||
let Mpu6000Event::Samples(batch) = complete(&mut driver, 411_300) else {
|
||||
panic!("checked batch");
|
||||
};
|
||||
|
||||
assert_eq!(sample.acceleration_milligravity, [1_000, -1_000, 0]);
|
||||
let sample = batch.as_slice()[0];
|
||||
assert_eq!(batch.as_slice().len(), 1);
|
||||
assert_eq!(sample.acceleration_milligravity, [1000, -1000, 0]);
|
||||
assert_eq!(
|
||||
sample.angular_rate_milliradians_per_second,
|
||||
[8_726, -8_726, 0]
|
||||
[8726, -8726, 0]
|
||||
);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, 3_653);
|
||||
assert_eq!(sample.timestamp, t(312_000));
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, 3653);
|
||||
assert_eq!(sample.timestamp, t(411_000));
|
||||
assert_eq!(complete(&mut driver, 411_400), Mpu6000Event::NotPending);
|
||||
assert_eq!(driver.batch, None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn bounded_drain_leaves_excess_and_partial_bytes() {
|
||||
for count in [14, 29, 1024] {
|
||||
let mut driver = initialized();
|
||||
begin_drain(&mut driver, count, 411_000);
|
||||
let packets = (usize::from(count) / 14).min(ImuBatch::CAPACITY);
|
||||
for index in 0..packets {
|
||||
assert_eq!(driver.bus.transmitted[0], 0xf4);
|
||||
response(
|
||||
&mut driver,
|
||||
if index + 1 == packets { &[0] } else { &BURST },
|
||||
);
|
||||
assert_eq!(
|
||||
complete(&mut driver, 411_100 + index as u64 * 100),
|
||||
Mpu6000Event::InProgress
|
||||
);
|
||||
}
|
||||
assert_eq!(driver.bus.transmitted[0], 0xba);
|
||||
response(&mut driver, &[0x50]);
|
||||
assert_eq!(complete(&mut driver, 412_000), Mpu6000Event::InProgress);
|
||||
let Mpu6000Event::Samples(batch) = complete(&mut driver, 412_100) else {
|
||||
panic!("checked batch");
|
||||
};
|
||||
assert_eq!(batch.as_slice().len(), packets);
|
||||
assert_eq!(driver.progress_at, Some(t(412_100)));
|
||||
assert!(driver.is_ready());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn empty_or_partial_fifo_cannot_renew_progress() {
|
||||
for count in [0, 1, 13] {
|
||||
let mut driver = initialized();
|
||||
assert_eq!(
|
||||
begin_drain(&mut driver, count, 411_000),
|
||||
Mpu6000Event::NotPending
|
||||
);
|
||||
assert_eq!(driver.bus.transmitted[0], 0xf2);
|
||||
assert!(driver.is_ready());
|
||||
assert_eq!(driver.progress_at, Some(t(410_000)));
|
||||
begin_drain(&mut driver, count, 439_999);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(440_000)),
|
||||
Mpu6000Event::Fault(Error::DataReadyTimeout)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn impossible_count_and_overflow_discard_pending_records() {
|
||||
let mut driver = initialized();
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(312_300)),
|
||||
Mpu6000Event::NotPending
|
||||
begin_drain(&mut driver, 1025, 411_000),
|
||||
Mpu6000Event::Fault(Error::FifoCount(1025))
|
||||
);
|
||||
for operation in [Operation::StartFifo, Operation::FifoStatus] {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
response(&mut driver, &[0x10]);
|
||||
driver.queue(operation, t(0));
|
||||
driver.submit_pending(t(0)).unwrap();
|
||||
assert_eq!(
|
||||
complete(&mut driver, 1),
|
||||
Mpu6000Event::Fault(Error::FifoOverflow)
|
||||
);
|
||||
assert_eq!(driver.batch, None);
|
||||
}
|
||||
let mut driver = initialized();
|
||||
begin_drain(&mut driver, 14, 411_000);
|
||||
response(&mut driver, &[0x10]);
|
||||
complete(&mut driver, 411_001);
|
||||
assert!(driver.batch.is_some());
|
||||
assert_eq!(
|
||||
complete(&mut driver, 411_002),
|
||||
Mpu6000Event::Fault(Error::FifoOverflow)
|
||||
);
|
||||
assert_eq!(driver.batch, None);
|
||||
assert_eq!(complete(&mut driver, 411_003), Mpu6000Event::NotPending);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reports_a_route_transfer_fault() {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.response[1] = 0x68;
|
||||
let mut driver = Mpu6000::new(bus, TEST_CONFIG);
|
||||
|
||||
start(&mut driver);
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::TransferFault, t(100)),
|
||||
Mpu6000Event::Fault(Error::TransferFault)
|
||||
));
|
||||
fn reset_during_drain_cannot_publish_repeated_fifo_bytes() {
|
||||
let mut driver = initialized();
|
||||
driver.request_samples(t(411_000)).unwrap();
|
||||
response(&mut driver, &14u16.to_be_bytes());
|
||||
complete(&mut driver, 411_000);
|
||||
response(&mut driver, &[0xaa; 14]);
|
||||
complete(&mut driver, 411_000);
|
||||
response(&mut driver, &[0]);
|
||||
complete(&mut driver, 411_001);
|
||||
assert!(driver.batch.is_some());
|
||||
// Reset clears both overflow status and FIFO enable. A clean status
|
||||
// alone must not accept bytes read after the buffer became disabled.
|
||||
assert_eq!(complete(&mut driver, 411_002), Mpu6000Event::InProgress);
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0xea, 0]);
|
||||
assert_eq!(
|
||||
complete(&mut driver, 411_003),
|
||||
Mpu6000Event::Fault(Error::FifoControl(0))
|
||||
);
|
||||
assert_eq!(driver.batch, None);
|
||||
assert_eq!(driver.progress_at, None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn completion_errors_never_republish_a_previous_sample() {
|
||||
fn completion_faults_discard_unpublished_records() {
|
||||
for (event, error) in [
|
||||
(
|
||||
SpiCompletion::Complete,
|
||||
@@ -551,74 +781,53 @@ mod tests {
|
||||
),
|
||||
(SpiCompletion::TransferFault, None),
|
||||
] {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.state = State::Ready { since: t(0) };
|
||||
driver.request_sample(t(1)).unwrap();
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(1)),
|
||||
Mpu6000Event::Sample(_)
|
||||
));
|
||||
driver.request_sample(t(2)).unwrap();
|
||||
let mut driver = initialized();
|
||||
begin_drain(&mut driver, 14, 411_000);
|
||||
response(&mut driver, &[0]);
|
||||
complete(&mut driver, 411_001);
|
||||
assert!(driver.batch.is_some());
|
||||
driver.receive.fill(0xa5);
|
||||
driver.bus.fail_completion = error;
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(event, t(2)),
|
||||
driver.on_transfer_complete(event, t(411_002)),
|
||||
Mpu6000Event::Fault(_)
|
||||
));
|
||||
assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]);
|
||||
assert!(!driver.is_ready());
|
||||
assert!(!matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(3)),
|
||||
Mpu6000Event::Sample(_)
|
||||
));
|
||||
if error == Some(CompletionError::Quarantined) {
|
||||
assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady));
|
||||
}
|
||||
assert_eq!(driver.batch, None);
|
||||
assert_eq!(complete(&mut driver, 411_003), Mpu6000Event::NotPending);
|
||||
assert_eq!(
|
||||
driver.begin_initialization(t(411_004)),
|
||||
Err(Error::NotReady)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejected_submission_cannot_complete_a_stale_sample() {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.state = State::Ready { since: t(0) };
|
||||
driver.receive.fill(0xa5);
|
||||
fn rejected_submission_discards_the_batch() {
|
||||
let mut driver = initialized();
|
||||
begin_drain(&mut driver, 14, 411_000);
|
||||
driver.bus.fail_start = true;
|
||||
assert_eq!(
|
||||
driver.request_sample(t(1)),
|
||||
Err(Error::Start(FakeError::Start))
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(2)),
|
||||
Mpu6000Event::NotPending
|
||||
complete(&mut driver, 411_001),
|
||||
Mpu6000Event::Fault(Error::Start(FakeError::Start))
|
||||
);
|
||||
assert_eq!(driver.batch, None);
|
||||
assert_eq!(complete(&mut driver, 411_002), Mpu6000Event::NotPending);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_transfer_phase_times_out_and_cancels_once_without_rearming() {
|
||||
let operations = [
|
||||
Operation::Reset,
|
||||
Operation::ResetSignalPaths,
|
||||
Operation::Identity,
|
||||
Operation::Sample,
|
||||
];
|
||||
for operation in operations
|
||||
.into_iter()
|
||||
.chain((0..9).map(Operation::Configure))
|
||||
{
|
||||
fn every_transfer_times_out_and_cancels_once() {
|
||||
for operation in operations() {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.queue(operation, t(100));
|
||||
driver.submit_pending(t(100)).unwrap();
|
||||
assert_eq!(driver.on_periodic_tick(t(10_099)), Mpu6000Event::NotPending);
|
||||
assert_eq!(driver.bus.cancellations, 0);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(10_100)),
|
||||
Mpu6000Event::Fault(Error::TransferTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 1);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)),
|
||||
Mpu6000Event::NotPending
|
||||
);
|
||||
assert_eq!(complete(&mut driver, 10_101), Mpu6000Event::NotPending);
|
||||
driver.on_periodic_tick(t(1_000_000));
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
assert_eq!(
|
||||
@@ -629,20 +838,13 @@ mod tests {
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() {
|
||||
for operation in [
|
||||
Operation::Reset,
|
||||
Operation::ResetSignalPaths,
|
||||
Operation::Identity,
|
||||
Operation::Sample,
|
||||
]
|
||||
.into_iter()
|
||||
.chain((0..9).map(Operation::Configure))
|
||||
{
|
||||
fn busy_admission_preserves_the_operation_and_expires() {
|
||||
for operation in operations() {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.bus.busy = true;
|
||||
driver.queue(operation, t(100));
|
||||
driver.submit_pending(t(100)).unwrap();
|
||||
driver.on_periodic_tick(t(200));
|
||||
assert_eq!(
|
||||
driver.state,
|
||||
State::Pending {
|
||||
@@ -650,7 +852,6 @@ mod tests {
|
||||
since: t(100)
|
||||
}
|
||||
);
|
||||
driver.on_periodic_tick(t(200));
|
||||
driver.bus.busy = false;
|
||||
driver.on_periodic_tick(t(300));
|
||||
assert_eq!(
|
||||
@@ -662,38 +863,53 @@ mod tests {
|
||||
);
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
}
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
let mut driver = initialized();
|
||||
driver.bus.busy = true;
|
||||
start(&mut driver);
|
||||
driver.request_samples(t(411_000)).unwrap();
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(20_000)),
|
||||
driver.on_periodic_tick(t(431_000)),
|
||||
Mpu6000Event::Fault(Error::AdmissionTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn missed_drdy_and_late_completions_fault_instead_of_polling_or_publishing() {
|
||||
fn late_completion_cannot_publish() {
|
||||
let mut driver = initialized();
|
||||
driver.on_periodic_tick(t(341_199));
|
||||
assert_eq!(driver.bus.starts, 12);
|
||||
begin_drain(&mut driver, 14, 411_000);
|
||||
response(&mut driver, &[0]);
|
||||
complete(&mut driver, 411_100);
|
||||
assert!(driver.batch.is_some());
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(341_200)),
|
||||
Mpu6000Event::Fault(Error::DataReadyTimeout)
|
||||
);
|
||||
let mut driver = initialized();
|
||||
driver.request_sample(t(312_000)).unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(322_000)),
|
||||
complete(&mut driver, 421_100),
|
||||
Mpu6000Event::Fault(Error::TransferTimeout)
|
||||
);
|
||||
assert_eq!(driver.batch, None);
|
||||
assert_eq!(driver.bus.cancellations, 1);
|
||||
let mut driver = initialized();
|
||||
driver.request_sample(t(312_000)).unwrap();
|
||||
driver.bus.fail_completion = Some(CompletionError::Quarantined);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(322_000)),
|
||||
Mpu6000Event::Fault(Error::Completion(CompletionError::Quarantined))
|
||||
driver.request_samples(t(440_000)),
|
||||
Err(Error::DataReadyTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined));
|
||||
assert!(!driver.is_ready());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn progress_expiry_cancels_active_drain() {
|
||||
for completion_error in [None, Some(CompletionError::Quarantined)] {
|
||||
let mut driver = initialized();
|
||||
begin_drain(&mut driver, 14, 439_000);
|
||||
driver.bus.fail_completion = completion_error;
|
||||
let expected = completion_error
|
||||
.map(Error::Completion)
|
||||
.unwrap_or(Error::DataReadyTimeout);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(440_000)),
|
||||
Mpu6000Event::Fault(expected)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 1);
|
||||
assert_eq!(driver.batch, None);
|
||||
assert_eq!(complete(&mut driver, 440_001), Mpu6000Event::NotPending);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -11,12 +11,19 @@ pub(crate) const GYRO_CONFIG: u8 = 0x1B;
|
||||
pub(crate) const ACCEL_CONFIG: u8 = 0x1C;
|
||||
pub(crate) const INT_PIN_CFG: u8 = 0x37;
|
||||
pub(crate) const INT_ENABLE: u8 = 0x38;
|
||||
pub(crate) const ACCEL_XOUT_H: u8 = 0x3B;
|
||||
pub(crate) const INT_STATUS: u8 = 0x3A;
|
||||
pub(crate) const FIFO_EN: u8 = 0x23;
|
||||
pub(crate) const FIFO_COUNT_H: u8 = 0x72;
|
||||
pub(crate) const FIFO_R_W: u8 = 0x74;
|
||||
|
||||
pub(crate) const CLOCK_SOURCE_PLL_X_GYRO: u8 = 0x01;
|
||||
pub(crate) const DEVICE_RESET: u8 = 0x80;
|
||||
pub(crate) const I2C_INTERFACE_DISABLE: u8 = 0x10;
|
||||
pub(crate) const ENABLE_ALL_SENSOR_AXES: u8 = 0x00;
|
||||
pub(crate) const INTERRUPT_CLEAR_ON_ANY_READ: u8 = 0x10;
|
||||
pub(crate) const DATA_READY_INTERRUPT: u8 = 0x01;
|
||||
pub(crate) const FIFO_OVERFLOW_INTERRUPT: u8 = 0x10;
|
||||
pub(crate) const FIFO_RESET: u8 = 0x04;
|
||||
pub(crate) const FIFO_ENABLE: u8 = 0x40;
|
||||
pub(crate) const FIFO_SENSOR_CHANNELS: u8 = 0xF8;
|
||||
pub(crate) const FIFO_CAPACITY: usize = 1024;
|
||||
pub(crate) const SAMPLE_BYTES: usize = 14;
|
||||
@@ -1,7 +1,7 @@
|
||||
//! Timestamped MPU6000 acquisition service.
|
||||
|
||||
use super::{Error, Mpu6000, Mpu6000Event};
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use crate::ImuBatch;
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use kaptan_time::MonotonicClock;
|
||||
|
||||
@@ -12,16 +12,13 @@ pub enum Mpu6000ServiceEvent<BusError> {
|
||||
Idle,
|
||||
/// MPU configuration completed and the DRDY route may be enabled.
|
||||
Initialized,
|
||||
/// A coherent IMU observation is ready for publication or control.
|
||||
Sample(ImuSnapshot),
|
||||
/// A bounded, overflow-checked FIFO drain is ready for publication.
|
||||
Samples(ImuBatch),
|
||||
/// The MPU6000 path entered a terminal fault state.
|
||||
Fault(Error<BusError>),
|
||||
}
|
||||
|
||||
/// Owns MPU6000 state, its clock, and sample acceptance.
|
||||
///
|
||||
/// Exactly one platform worker invokes this service. Hardware ISRs only report
|
||||
/// deferred DRDY, timer, and SPI-DMA terminal events to that worker.
|
||||
/// Drives MPU6000 acquisition from deferred events using one monotonic clock.
|
||||
pub struct Mpu6000Service<Clock, Bus> {
|
||||
clock: Clock,
|
||||
imu: Mpu6000<Bus>,
|
||||
@@ -49,14 +46,13 @@ where
|
||||
service_event(self.imu.on_periodic_tick(self.clock.now()))
|
||||
}
|
||||
|
||||
/// Handles one coalesced data-ready notification. Time is captured here in
|
||||
/// the worker, at read submission, not at IRQ entry or physical sampling.
|
||||
/// Starts one bounded drain after a coalesced data-ready notification.
|
||||
pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent<Bus::StartError> {
|
||||
if !self.imu.is_ready() {
|
||||
return Mpu6000ServiceEvent::Idle;
|
||||
}
|
||||
|
||||
match self.imu.request_sample(self.clock.now()) {
|
||||
match self.imu.request_samples(self.clock.now()) {
|
||||
Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle,
|
||||
Err(error) => Mpu6000ServiceEvent::Fault(error),
|
||||
}
|
||||
@@ -73,32 +69,23 @@ where
|
||||
|
||||
fn service_event<E>(event: Mpu6000Event<E>) -> Mpu6000ServiceEvent<E> {
|
||||
match event {
|
||||
Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle,
|
||||
Mpu6000Event::NotPending | Mpu6000Event::InProgress => Mpu6000ServiceEvent::Idle,
|
||||
Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized,
|
||||
Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample),
|
||||
Mpu6000Event::Samples(batch) => Mpu6000ServiceEvent::Samples(batch),
|
||||
Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{Mpu6000Service, Mpu6000ServiceEvent};
|
||||
use crate::mpu6000::test_bus::FakeBus;
|
||||
use super::*;
|
||||
use crate::mpu6000::{
|
||||
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config,
|
||||
};
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use kaptan_time::{MonotonicClock, Timestamp};
|
||||
|
||||
const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||
gyroscope: GyroscopeRange::PlusMinus1000DegreesPerSecond,
|
||||
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
|
||||
sample_rate_divider: 9,
|
||||
test_bus::FakeBus,
|
||||
tests::{TEST_CONFIG, initialized},
|
||||
};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
struct Clock(u64);
|
||||
|
||||
impl MonotonicClock for Clock {
|
||||
fn now(&mut self) -> Timestamp {
|
||||
Timestamp::from_microseconds(self.0)
|
||||
@@ -106,56 +93,47 @@ mod tests {
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn accepts_a_sample_only_after_initialization() {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.response[1] = 0x68;
|
||||
let mut service = Mpu6000Service::new(Clock(0), Mpu6000::new(bus, TEST_CONFIG));
|
||||
|
||||
service.begin().unwrap();
|
||||
fn publication_waits_for_post_status() {
|
||||
let mut service = Mpu6000Service::new(Clock(411_000), initialized());
|
||||
assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
|
||||
service.clock.0 = 99_999;
|
||||
service.on_periodic_tick();
|
||||
assert_eq!(service.imu.bus.starts, 0);
|
||||
service.clock.0 = 100_000;
|
||||
service.imu.bus.response[1..3].copy_from_slice(&14u16.to_be_bytes());
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.imu.bus.response.fill(0);
|
||||
service.clock.0 = 411_100;
|
||||
service.imu.bus.busy = true;
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.clock.0 = 411_200;
|
||||
service.imu.bus.busy = false;
|
||||
service.on_periodic_tick();
|
||||
service.clock.0 = 411_300;
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
service.clock.0 = 200_000;
|
||||
service.on_periodic_tick();
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.clock.0 = 300_000;
|
||||
service.on_periodic_tick();
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.clock.0 = 400_000;
|
||||
service.on_periodic_tick();
|
||||
for _ in 1..9 {
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
}
|
||||
service.clock.0 = 410_000;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Mpu6000ServiceEvent::Initialized
|
||||
));
|
||||
service.clock.0 = 410_042;
|
||||
assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
|
||||
service.clock.0 = 410_123;
|
||||
service.clock.0 = 411_400;
|
||||
service.imu.bus.response[1] = 0x50;
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(410_042)
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
service.clock.0 = 411_500;
|
||||
let Mpu6000ServiceEvent::Samples(batch) =
|
||||
service.on_transfer_complete(SpiCompletion::Complete)
|
||||
else {
|
||||
panic!("checked batch");
|
||||
};
|
||||
assert_eq!(
|
||||
batch.as_slice()[0].timestamp,
|
||||
Timestamp::from_microseconds(411_200)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn fake_clock_expires_active_work_without_any_irq() {
|
||||
fn clock_expires_active_work_without_an_irq() {
|
||||
let mut service =
|
||||
Mpu6000Service::new(Clock(50), Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
|
||||
service.begin().unwrap();
|
||||
assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
|
||||
service.clock.0 = 100_050;
|
||||
service.on_periodic_tick();
|
||||
service.clock.0 = 110_049;
|
||||
@@ -166,7 +144,7 @@ mod tests {
|
||||
service.clock.0 = 110_050;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Mpu6000ServiceEvent::Fault(super::Error::TransferTimeout)
|
||||
Mpu6000ServiceEvent::Fault(Error::TransferTimeout)
|
||||
));
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
|
||||
@@ -11,27 +11,27 @@ pub(crate) enum FakeError {
|
||||
}
|
||||
|
||||
pub(crate) struct FakeBus {
|
||||
pub response: [u8; 15],
|
||||
pub response: [u8; 32],
|
||||
pub fail_start: bool,
|
||||
pub fail_completion: Option<CompletionError>,
|
||||
pub busy: bool,
|
||||
pub starts: usize,
|
||||
pub cancellations: usize,
|
||||
pub transmitted: [u8; 15],
|
||||
pending: Option<([u8; 15], usize)>,
|
||||
pub transmitted: [u8; 32],
|
||||
pending: Option<([u8; 32], usize)>,
|
||||
quarantined: bool,
|
||||
}
|
||||
|
||||
impl FakeBus {
|
||||
pub fn ready() -> Self {
|
||||
Self {
|
||||
response: [0; 15],
|
||||
response: [0; 32],
|
||||
fail_start: false,
|
||||
fail_completion: None,
|
||||
busy: false,
|
||||
starts: 0,
|
||||
cancellations: 0,
|
||||
transmitted: [0; 15],
|
||||
transmitted: [0; 32],
|
||||
pending: None,
|
||||
quarantined: false,
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user