embedded_sensors/mpu925x/
mod.rs

1use core::marker::PhantomData;
2
3use config::Config;
4use embedded_hal::blocking::{delay, i2c};
5use nalgebra::{Quaternion, Vector3};
6
7use self::result::{Error, Result};
8use crate::{ak8963, mpu6500};
9
10pub type Madgwick = ahrs::Madgwick<f32>;
11pub type Mahony = ahrs::Mahony<f32>;
12
13pub mod config;
14pub mod result;
15
16const MADGWICK_BETA: f32 = 0.60459978807; // sqrt(3.0 / 4.0) * PI * (40.0 / 180.0)
17
18const MAHONY_KP: f32 = 10.0;
19const MAHONY_KI: f32 = 0.0;
20
21pub struct Mpu925x<I2C, AHRS>
22where
23    I2C: i2c::Read + i2c::Write,
24    AHRS: ahrs::Ahrs<f32>,
25{
26    ak: ak8963::Ak8963<I2C>,
27    mpu: mpu6500::Mpu6500<I2C>,
28    ahrs: AHRS,
29    _i2c: PhantomData<I2C>,
30}
31
32impl<I2C, AHRS, I2cError> Mpu925x<I2C, AHRS>
33where
34    I2C: i2c::Read<Error = I2cError> + i2c::Write<Error = I2cError>,
35    AHRS: ahrs::Ahrs<f32>,
36{
37    fn with_configuration_ahrs<DELAY>(
38        ak_addr: u8,
39        mpu_addr: u8,
40        i2c: &mut I2C,
41        delay: &mut DELAY,
42        cfg: Config,
43        ahrs: AHRS,
44    ) -> Result<Self, I2cError>
45    where
46        DELAY: delay::DelayMs<u16>,
47    {
48        let mpu = match mpu6500::Mpu6500::with_configuration(mpu_addr, i2c, delay, cfg.mpu) {
49            Ok(v) => v,
50            Err(e) => return Err(Error::Mpu6500Error(e)),
51        };
52
53        let ak = match ak8963::Ak8963::with_configuration(ak_addr, i2c, delay, cfg.ak) {
54            Ok(v) => v,
55            Err(e) => return Err(Error::Ak8963Error(e)),
56        };
57
58        Ok(Self {
59            ak,
60            mpu,
61            ahrs,
62            _i2c: PhantomData::default(),
63        })
64    }
65
66    pub fn read(&mut self, i2c: &mut I2C) -> Result<(), I2cError> {
67        if let Err(e) = self.mpu.read_imu(i2c) {
68            return Err(Error::Mpu6500Error(e));
69        }
70
71        if let Err(e) = self.ak.read(i2c) {
72            return Err(Error::Ak8963Error(e));
73        }
74
75        match self.ahrs.update(
76            &self.mpu.angular_velocity,
77            &self.mpu.acceleration,
78            &self.ak.magnetic_field,
79        ) {
80            Ok(_) => {}
81            Err(e) => {
82                return match e {
83                    "Accelerometer norm divided by zero." => Err(Error::AhrsUpdateAccelerometer),
84                    "Magnetometer norm divided by zero." => Err(Error::AhrsUpdateMagnetometer),
85                    _ => unreachable!(),
86                }
87            }
88        }
89
90        Ok(())
91    }
92
93    pub fn acceleration(&self) -> Vector3<f32> {
94        self.mpu.acceleration
95    }
96
97    pub fn angular_velocity(&self) -> Vector3<f32> {
98        self.mpu.angular_velocity
99    }
100
101    pub fn magnetic_field(&self) -> Vector3<f32> {
102        self.ak.magnetic_field
103    }
104}
105
106impl<I2C, I2cError> Mpu925x<I2C, Madgwick>
107where
108    I2C: i2c::Read<Error = I2cError> + i2c::Write<Error = I2cError>,
109{
110    pub fn new<DELAY>(
111        ak_addr: u8,
112        mpu_addr: u8,
113        i2c: &mut I2C,
114        delay: &mut DELAY,
115    ) -> Result<Self, I2cError>
116    where
117        DELAY: delay::DelayMs<u16>,
118    {
119        Self::with_configuration(ak_addr, mpu_addr, i2c, delay, config::Config::default())
120    }
121
122    pub fn with_configuration<DELAY>(
123        ak_addr: u8,
124        mpu_addr: u8,
125        i2c: &mut I2C,
126        delay: &mut DELAY,
127        cfg: Config,
128    ) -> Result<Self, I2cError>
129    where
130        DELAY: delay::DelayMs<u16>,
131    {
132        let sample_rate_freq = cfg.mpu.fifo_sample_rate.get_freq();
133        let sample_period = sample_rate_freq as f32 / 1000.0;
134
135        Self::with_configuration_ahrs(
136            ak_addr,
137            mpu_addr,
138            i2c,
139            delay,
140            cfg,
141            ahrs::Madgwick::new(sample_period, MADGWICK_BETA),
142        )
143    }
144
145    pub fn calibrate_mpu<DELAY>(&mut self, i2c: &mut I2C, delay: &mut DELAY) -> Result<(), I2cError>
146    where
147        DELAY: delay::DelayMs<u16>,
148    {
149        match self.mpu.calibrate(i2c, delay) {
150            Ok(()) => Ok(()),
151            Err(e) => Err(Error::Mpu6500Error(e)),
152        }
153    }
154
155    pub fn calibrate_ak<DELAY>(&mut self, i2c: &mut I2C, delay: &mut DELAY) -> Result<(), I2cError>
156    where
157        DELAY: delay::DelayMs<u16>,
158    {
159        match self.ak.calibrate(i2c, delay) {
160            Ok(()) => Ok(()),
161            Err(e) => Err(Error::Ak8963Error(e)),
162        }
163    }
164
165    pub fn rotation(&self) -> Quaternion<f32> {
166        self.ahrs.quat
167    }
168}
169
170impl<I2C, I2cError> Mpu925x<I2C, Mahony>
171where
172    I2C: i2c::Read<Error = I2cError> + i2c::Write<Error = I2cError>,
173{
174    pub fn new<DELAY>(
175        ak_addr: u8,
176        mpu_addr: u8,
177        i2c: &mut I2C,
178        delay: &mut DELAY,
179    ) -> Result<Self, I2cError>
180    where
181        DELAY: delay::DelayMs<u16>,
182    {
183        Self::with_configuration(ak_addr, mpu_addr, i2c, delay, config::Config::default())
184    }
185
186    pub fn with_configuration<DELAY>(
187        ak_addr: u8,
188        mpu_addr: u8,
189        i2c: &mut I2C,
190        delay: &mut DELAY,
191        cfg: Config,
192    ) -> Result<Self, I2cError>
193    where
194        DELAY: delay::DelayMs<u16>,
195    {
196        let sample_rate_freq = cfg.mpu.fifo_sample_rate.get_freq();
197        let sample_period = sample_rate_freq as f32 / 1000.0;
198
199        Self::with_configuration_ahrs(
200            ak_addr,
201            mpu_addr,
202            i2c,
203            delay,
204            cfg,
205            ahrs::Mahony::new(sample_period, MAHONY_KP, MAHONY_KI),
206        )
207    }
208
209    pub fn rotation(&self) -> Quaternion<f32> {
210        self.ahrs.quat
211    }
212}
embedded_sensors/mpu925x/mod.rs

embedded_sensors/mpu925x/
mod.rs
