embedded_sensors/ak8963/
mod.rs

1use core::marker::PhantomData;
2
3use embedded_hal::blocking::{delay, i2c};
4use nalgebra::Vector3;
5
6use self::{
7    config::Config,
8    register::Register,
9    result::{Error, Result},
10};
11
12pub mod config;
13mod register;
14pub mod result;
15
16pub const I2C_ADDR: u8 = 0x0C;
17pub const DEV_ID: u8 = 0x48;
18
19pub struct Ak8963<I2C>
20where
21    I2C: i2c::Read + i2c::Write,
22{
23    addr: u8,
24    cfg: Config,
25    pub(crate) magnetic_field: Vector3<f32>,
26    mag_resolution: f32,
27    mag_bias_factory: Vector3<f32>,
28    mag_bias: Vector3<f32>,
29    mag_scale: Vector3<f32>,
30    _i2c: PhantomData<I2C>,
31}
32
33impl<I2C, I2cError> Ak8963<I2C>
34where
35    I2C: i2c::Read<Error = I2cError> + i2c::Write<Error = I2cError>,
36{
37    pub fn new<DELAY>(addr: u8, delay: &mut DELAY, i2c: &mut I2C) -> Result<Self, I2cError>
38    where
39        DELAY: delay::DelayMs<u16>,
40    {
41        Self::with_configuration(addr, i2c, delay, Config::default())
42    }
43
44    pub fn with_configuration<DELAY>(
45        addr: u8,
46        i2c: &mut I2C,
47        delay: &mut DELAY,
48        cfg: Config,
49    ) -> Result<Self, I2cError>
50    where
51        DELAY: delay::DelayMs<u16>,
52    {
53        let dev_id = Self::who_am_i(addr, i2c)?;
54
55        if dev_id != DEV_ID {
56            return Err(Error::InvalidDevice(dev_id));
57        }
58
59        let mut ak = Self {
60            addr,
61            magnetic_field: Vector3::default(),
62            mag_scale: Vector3::new(1.0, 1.0, 1.0),
63            mag_bias: Vector3::default(),
64            mag_bias_factory: Vector3::default(),
65            mag_resolution: cfg.output_bits.get_resolution(),
66            cfg,
67            _i2c: PhantomData::default(),
68        };
69
70        ak.init(i2c, delay)?;
71
72        Ok(ak)
73    }
74
75    fn init<DELAY>(&mut self, i2c: &mut I2C, delay: &mut DELAY) -> Result<(), I2cError>
76    where
77        DELAY: delay::DelayMs<u16>,
78    {
79        Self::write_register(self.addr, i2c, Register::CNTL, 0x00)?; // shutdown
80        delay.delay_ms(10);
81
82        Self::write_register(self.addr, i2c, Register::CNTL, 0x0F)?; // enter fuse rom access mode
83        delay.delay_ms(10);
84
85        let mut raw = [0; 3];
86        Self::read_register(self.addr, i2c, Register::ASAX, &mut raw)?; // read axis calibration values
87
88        self.mag_bias_factory = Vector3::new(
89            (raw[0] - 128) as f32 / 256.0 + 1.0,
90            (raw[1] - 128) as f32 / 256.0 + 1.0,
91            (raw[2] - 128) as f32 / 256.0 + 1.0,
92        );
93
94        Self::write_register(self.addr, i2c, Register::CNTL, 0x00)?; // shutdown
95        delay.delay_ms(10);
96
97        Self::write_register(
98            self.addr,
99            i2c,
100            Register::CNTL,
101            (self.cfg.output_bits as u8) << 4 | (self.cfg.measurement_mode as u8),
102        )?; // set data resolution and sample odr
103        delay.delay_ms(10);
104
105        Ok(())
106    }
107
108    pub fn read(&mut self, i2c: &mut I2C) -> Result<(), I2cError> {
109        let raw = Self::read_raw(self.addr, i2c)?;
110
111        let bias_to_current_bias =
112            self.mag_resolution / config::OutputBits::Bits16.get_resolution();
113
114        self.magnetic_field = Vector3::new(
115            (raw.x as f32 * self.mag_resolution * self.mag_bias_factory.x
116                - self.mag_bias.x * bias_to_current_bias)
117                * self.mag_scale.x,
118            (raw.y as f32 * self.mag_resolution * self.mag_bias_factory.y
119                - self.mag_bias.y * bias_to_current_bias)
120                * self.mag_scale.y,
121            (raw.z as f32 * self.mag_resolution * self.mag_bias_factory.z
122                - self.mag_bias.z * bias_to_current_bias)
123                * self.mag_scale.z,
124        );
125
126        Ok(())
127    }
128
129    pub fn calibrate<DELAY>(&mut self, i2c: &mut I2C, delay: &mut DELAY) -> Result<(), I2cError>
130    where
131        DELAY: delay::DelayMs<u16>,
132    {
133        let output_bits = self.cfg.output_bits;
134
135        self.cfg.output_bits = config::OutputBits::Bits16;
136        self.init(i2c, delay)?;
137
138        delay.delay_ms(4000);
139
140        let sample_count = match self.cfg.measurement_mode {
141            config::MeasurementMode::ContinuousMeasurement8Hz => 128,
142            config::MeasurementMode::ContinuousMeasurement100Hz => 1500,
143            _ => 0,
144        };
145
146        let mut max = Vector3::new(i16::MIN, i16::MIN, i16::MIN);
147        let mut min = Vector3::new(i16::MAX, i16::MAX, i16::MAX);
148
149        for _ in 0..sample_count {
150            let raw = Self::read_raw(self.addr, i2c)?;
151
152            max.x = i16::max(max.x, raw.x);
153            min.x = i16::min(min.x, raw.x);
154
155            max.y = i16::max(max.y, raw.y);
156            min.y = i16::min(min.y, raw.y);
157
158            max.z = i16::max(max.z, raw.z);
159            min.z = i16::min(min.z, raw.z);
160
161            match self.cfg.measurement_mode {
162                config::MeasurementMode::ContinuousMeasurement8Hz => delay.delay_ms(135),
163                config::MeasurementMode::ContinuousMeasurement100Hz => delay.delay_ms(12),
164                _ => {}
165            }
166        }
167
168        let bias = (max + min) / 2;
169        let scale = (max - min) / 2;
170
171        let bias_resolution = config::OutputBits::Bits16.get_resolution();
172        self.mag_bias = Vector3::new(
173            bias.x as f32 * bias_resolution * self.mag_bias_factory.x,
174            bias.y as f32 * bias_resolution * self.mag_bias_factory.y,
175            bias.z as f32 * bias_resolution * self.mag_bias_factory.z,
176        );
177
178        let avg_rad = (scale.x + scale.y + scale.z) as f32 / 3.0;
179        self.mag_scale = Vector3::new(
180            avg_rad / scale.x as f32,
181            avg_rad / scale.y as f32,
182            avg_rad / scale.z as f32,
183        );
184
185        self.cfg.output_bits = output_bits;
186        self.init(i2c, delay)?;
187
188        Ok(())
189    }
190
191    pub fn magnetic_field(&self) -> Vector3<f32> {
192        self.magnetic_field
193    }
194
195    fn read_raw(addr: u8, i2c: &mut I2C) -> Result<Vector3<i16>, I2cError> {
196        let mut buf = [0; 1];
197        Self::read_register(addr, i2c, Register::ST1, &mut buf)?;
198
199        if (buf[0] & 0x01) == 0 {
200            return Err(Error::DataNotReady);
201        }
202
203        let mut buf = [0; 7];
204        Self::read_register(addr, i2c, Register::HXL, &mut buf)?;
205
206        if (buf[6] & 0x08) != 0 {
207            return Err(Error::Overflow);
208        }
209
210        Ok(Vector3::new(
211            i16::from_be_bytes([buf[1], buf[0]]),
212            i16::from_be_bytes([buf[3], buf[2]]),
213            i16::from_be_bytes([buf[5], buf[4]]),
214        ))
215    }
216
217    fn who_am_i(addr: u8, i2c: &mut I2C) -> Result<u8, I2cError> {
218        let mut buf = [0; 1];
219        Self::read_register(addr, i2c, Register::WIA, &mut buf)?;
220        Ok(buf[0])
221    }
222
223    fn read_register(
224        addr: u8,
225        i2c: &mut I2C,
226        reg: Register,
227        buf: &mut [u8],
228    ) -> Result<(), I2cError> {
229        match i2c.write(addr, &[reg as u8]) {
230            Ok(()) => {}
231            Err(e) => return Err(Error::I2cError(e)),
232        }
233
234        match i2c.read(addr, buf) {
235            Ok(()) => Ok(()),
236            Err(e) => Err(Error::I2cError(e)),
237        }
238    }
239
240    fn write_register(addr: u8, i2c: &mut I2C, reg: Register, cmd: u8) -> Result<(), I2cError> {
241        match i2c.write(addr, &[reg as u8, cmd]) {
242            Ok(()) => Ok(()),
243            Err(e) => Err(Error::I2cError(e)),
244        }
245    }
246}
embedded_sensors/ak8963/mod.rs

embedded_sensors/ak8963/
mod.rs
