cu_mpu9250/
lib.rs

1#![cfg_attr(not(feature = "std"), no_std)]
2
3extern crate alloc;
4
5use alloc::format;
6use core::fmt::Debug;
7
8use core::marker::PhantomData;
9pub use cu_sensor_payloads::ImuPayload;
10use cu29::prelude::*;
11use embedded_hal_1 as eh1;
12use mpu9250::{Device, Imu as ImuOnly, Marg, Mpu9250, NineDOFDevice};
13use serde::Deserialize;
14
15pub mod embedded_hal;
16
17pub use embedded_hal::set_gyro_bias;
18
19fn map_debug_error<E: Debug>(context: &str, err: E) -> CuError {
20    CuError::from(format!("{context}: {err:?}"))
21}
22
23#[derive(Serialize, Deserialize)]
24#[repr(u8)]
25pub enum WhoAmI {
26    Mpu9250 = 0x71,
27    Mpu9255 = 0x73,
28    Mpu6500 = 0x70,
29    Clone75 = 0x75,
30    Ak8963 = 0x48,
31    Unknown,
32}
33
34impl From<u8> for WhoAmI {
35    fn from(v: u8) -> Self {
36        match v {
37            0x71 => WhoAmI::Mpu9250,
38            0x73 => WhoAmI::Mpu9255,
39            0x70 => WhoAmI::Mpu6500,
40            0x75 => WhoAmI::Clone75,
41            0x48 => WhoAmI::Ak8963,
42            _ => WhoAmI::Unknown,
43        }
44    }
45}
46
47/// Trait implemented by MPU9250 backends capable of producing an [`ImuPayload`].
48pub trait Mpu9250Device: Send + 'static {
49    type Error: Debug;
50
51    fn who_am_i(&mut self) -> Result<WhoAmI, Self::Error>;
52    fn read_measure(&mut self) -> Result<ImuPayload, Self::Error>;
53}
54
55impl<DEV> Mpu9250Device for Mpu9250<DEV, Marg>
56where
57    DEV: Device + NineDOFDevice + Send + 'static,
58    <DEV as Device>::Error: Debug,
59{
60    type Error = <DEV as Device>::Error;
61
62    fn who_am_i(&mut self) -> Result<WhoAmI, Self::Error> {
63        Ok(WhoAmI::from(Mpu9250::who_am_i(self)?))
64    }
65
66    fn read_measure(&mut self) -> Result<ImuPayload, Self::Error> {
67        let data = self.all::<[f32; 3]>()?;
68        Ok(ImuPayload::from_raw(data.accel, data.gyro, data.temp))
69    }
70}
71
72impl<DEV> Mpu9250Device for Mpu9250<DEV, ImuOnly>
73where
74    DEV: Device + Send + 'static,
75    <DEV as Device>::Error: Debug,
76{
77    type Error = <DEV as Device>::Error;
78
79    fn who_am_i(&mut self) -> Result<WhoAmI, Self::Error> {
80        Ok(WhoAmI::from(Mpu9250::who_am_i(self)?))
81    }
82
83    fn read_measure(&mut self) -> Result<ImuPayload, Self::Error> {
84        let data = self.all::<[f32; 3]>()?;
85        Ok(ImuPayload::from_raw(data.accel, data.gyro, data.temp))
86    }
87}
88
89resources!(for <SPI, CS, D>
90where
91    SPI: eh1::spi::SpiBus<u8> + Send + Sync + 'static,
92    SPI::Error: Debug + Send + 'static,
93    CS: eh1::digital::OutputPin + Send + Sync + 'static,
94    CS::Error: Debug + Send + 'static,
95    D: eh1::delay::DelayNs + Send + Sync + 'static,
96{
97    spi => Owned<SPI>,
98    cs => Owned<CS>,
99    delay => Owned<D>,
100});
101
102/// Copper source task for the MPU9250.
103#[derive(Reflect)]
104#[reflect(no_field_bounds, from_reflect = false, type_path = false)]
105pub struct Mpu9250Source<SPI, CS, D>
106where
107    SPI: eh1::spi::SpiBus<u8> + Send + Sync + 'static,
108    SPI::Error: Debug + Send + 'static,
109    CS: eh1::digital::OutputPin + Send + Sync + 'static,
110    CS::Error: Debug + Send + 'static,
111    D: eh1::delay::DelayNs + Send + Sync + 'static,
112{
113    #[reflect(ignore)]
114    driver: embedded_hal::EmbeddedHalDriver<SPI, CS>,
115    #[reflect(ignore)]
116    _pd: PhantomData<fn() -> D>,
117}
118
119impl<SPI, CS, D> TypePath for Mpu9250Source<SPI, CS, D>
120where
121    SPI: eh1::spi::SpiBus<u8> + Send + Sync + 'static,
122    SPI::Error: Debug + Send + 'static,
123    CS: eh1::digital::OutputPin + Send + Sync + 'static,
124    CS::Error: Debug + Send + 'static,
125    D: eh1::delay::DelayNs + Send + Sync + 'static,
126{
127    fn type_path() -> &'static str {
128        "cu_mpu9250::Mpu9250Source"
129    }
130
131    fn short_type_path() -> &'static str {
132        "Mpu9250Source"
133    }
134
135    fn type_ident() -> Option<&'static str> {
136        Some("Mpu9250Source")
137    }
138
139    fn crate_name() -> Option<&'static str> {
140        Some("cu_mpu9250")
141    }
142
143    fn module_path() -> Option<&'static str> {
144        Some("")
145    }
146}
147
148impl<SPI, CS, D> Freezable for Mpu9250Source<SPI, CS, D>
149where
150    SPI: eh1::spi::SpiBus<u8> + Send + Sync + 'static,
151    SPI::Error: Debug + Send + 'static,
152    CS: eh1::digital::OutputPin + Send + Sync + 'static,
153    CS::Error: Debug + Send + 'static,
154    D: eh1::delay::DelayNs + Send + Sync + 'static,
155{
156}
157
158impl<SPI, CS, D> CuSrcTask for Mpu9250Source<SPI, CS, D>
159where
160    SPI: eh1::spi::SpiBus<u8> + Send + Sync + 'static,
161    SPI::Error: Debug + Send + 'static,
162    CS: eh1::digital::OutputPin + Send + Sync + 'static,
163    CS::Error: Debug + Send + 'static,
164    D: eh1::delay::DelayNs + Send + Sync + 'static,
165{
166    type Resources<'r> = Resources<SPI, CS, D>;
167    type Output<'m> = output_msg!(ImuPayload);
168
169    fn new(config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self> {
170        let settings = embedded_hal::EmbeddedHalSettings::from_config(config)?;
171        let driver = embedded_hal::EmbeddedHalDriver::new(
172            resources.spi.0,
173            resources.cs.0,
174            resources.delay.0,
175            settings,
176        )?;
177
178        Ok(Self {
179            driver,
180            _pd: PhantomData,
181        })
182    }
183
184    fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
185        self.driver
186            .who_am_i()
187            .map_err(|err| map_debug_error("mpu9250 WHO_AM_I", err))?;
188        Ok(())
189    }
190
191    fn process<'o>(&mut self, ctx: &CuContext, new_msg: &mut Self::Output<'o>) -> CuResult<()> {
192        let tov = ctx.now(); // best effort here
193        let payload = self
194            .driver
195            .read_measure()
196            .map_err(|err| map_debug_error("mpu9250 read", err))?;
197        new_msg.tov = Some(tov).into();
198        // TODO: Make a good short status message for the sensor.
199        // new_msg.metadata.set_status("ok");
200        new_msg.set_payload(payload);
201        Ok(())
202    }
203}
cu_mpu9250/lib.rs

cu_mpu9250/
lib.rs
