83 lines
2.9 KiB
Rust
83 lines
2.9 KiB
Rust
//! Timestamped ICM-42688-P acquisition service.
|
|
|
|
use super::{Error, Icm42688, Icm42688Event};
|
|
use kaptan_sensor::imu::ImuSnapshot;
|
|
use kaptan_spi::{SpiCompletion, SpiDevice};
|
|
use kaptan_time::MonotonicClock;
|
|
|
|
/// One observable result from an ICM-42688-P acquisition service.
|
|
#[derive(Debug)]
|
|
pub enum Icm42688ServiceEvent<BusError> {
|
|
/// No accepted sample or terminal fault was produced.
|
|
Idle,
|
|
/// IMU configuration completed and the DRDY route may be enabled.
|
|
Initialized,
|
|
/// A coherent IMU observation is ready for publication or control.
|
|
Sample(ImuSnapshot),
|
|
/// The ICM-42688-P path entered a terminal fault state.
|
|
Fault(Error<BusError>),
|
|
}
|
|
|
|
/// Owns ICM-42688-P state, its clock, and sample acceptance.
|
|
pub struct Icm42688Service<Clock, Bus> {
|
|
clock: Clock,
|
|
imu: Icm42688<Bus>,
|
|
}
|
|
|
|
impl<Clock, Bus> Icm42688Service<Clock, Bus> {
|
|
/// Creates an inactive ICM-42688-P acquisition service.
|
|
pub const fn new(clock: Clock, imu: Icm42688<Bus>) -> Self {
|
|
Self { clock, imu }
|
|
}
|
|
}
|
|
|
|
impl<Clock, Bus> Icm42688Service<Clock, Bus>
|
|
where
|
|
Clock: MonotonicClock,
|
|
Bus: SpiDevice,
|
|
{
|
|
/// Submits the asynchronous reset transaction.
|
|
pub fn begin(&mut self) -> Result<(), Error<Bus::StartError>> {
|
|
self.imu.begin_initialization()
|
|
}
|
|
|
|
/// Advances reset and sensor-start settling states on a periodic tick.
|
|
pub fn on_periodic_tick(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
|
|
if self.imu.is_ready() {
|
|
return match self.imu.request_sample(self.clock.now()) {
|
|
Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle,
|
|
Err(error) => Icm42688ServiceEvent::Fault(error),
|
|
};
|
|
}
|
|
let _ = self.clock.now();
|
|
match self.imu.advance_initialization() {
|
|
Ok(()) => Icm42688ServiceEvent::Idle,
|
|
Err(error) => Icm42688ServiceEvent::Fault(error),
|
|
}
|
|
}
|
|
|
|
/// Handles one coalesced data-ready event.
|
|
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()) {
|
|
Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle,
|
|
Err(error) => Icm42688ServiceEvent::Fault(error),
|
|
}
|
|
}
|
|
|
|
/// Advances the transaction state from a DMA terminal result.
|
|
pub fn on_transfer_complete(
|
|
&mut self,
|
|
completion: SpiCompletion,
|
|
) -> Icm42688ServiceEvent<Bus::StartError> {
|
|
match self.imu.on_transfer_complete(completion) {
|
|
Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle,
|
|
Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized,
|
|
Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
|
|
Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error),
|
|
}
|
|
}
|
|
}
|