1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
use crate::{
interface::{ReadData, WriteData},
mode, Error, Lsm303agr, MagOutputDataRate, Measurement, Register, UnscaledMeasurement,
};
impl<DI, CommE, PinE, MODE> Lsm303agr<DI, MODE>
where
DI: ReadData<Error = Error<CommE, PinE>> + WriteData<Error = Error<CommE, PinE>>,
{
pub fn set_mag_odr(&mut self, odr: MagOutputDataRate) -> Result<(), Error<CommE, PinE>> {
let cfg = self.cfg_reg_a_m.bits & 0xF3;
let mask = match odr {
MagOutputDataRate::Hz10 => 0,
MagOutputDataRate::Hz20 => 1 << 2,
MagOutputDataRate::Hz50 => 2 << 2,
MagOutputDataRate::Hz100 => 3 << 2,
};
self.iface
.write_mag_register(Register::CFG_REG_A_M, cfg | mask)?;
self.cfg_reg_a_m = (cfg | mask).into();
Ok(())
}
}
impl<DI, CommE, PinE> Lsm303agr<DI, mode::MagContinuous>
where
DI: ReadData<Error = Error<CommE, PinE>> + WriteData<Error = Error<CommE, PinE>>,
{
pub fn mag_data(&mut self) -> Result<Measurement, Error<CommE, PinE>> {
let unscaled = self.mag_data_unscaled()?;
Ok(Measurement {
x: scale_measurement(unscaled.x),
y: scale_measurement(unscaled.y),
z: scale_measurement(unscaled.z),
})
}
pub fn mag_data_unscaled(&mut self) -> Result<UnscaledMeasurement, Error<CommE, PinE>> {
let data = self
.iface
.read_mag_3_double_registers(Register::OUTX_L_REG_M)?;
Ok(UnscaledMeasurement {
x: data.0 as i16,
y: data.1 as i16,
z: data.2 as i16,
})
}
}
impl<DI, CommE, PinE> Lsm303agr<DI, mode::MagOneShot>
where
DI: ReadData<Error = Error<CommE, PinE>> + WriteData<Error = Error<CommE, PinE>>,
{
pub fn mag_data(&mut self) -> nb::Result<Measurement, Error<CommE, PinE>> {
let unscaled = self.mag_data_unscaled()?;
Ok(Measurement {
x: scale_measurement(unscaled.x),
y: scale_measurement(unscaled.y),
z: scale_measurement(unscaled.z),
})
}
pub fn mag_data_unscaled(&mut self) -> nb::Result<UnscaledMeasurement, Error<CommE, PinE>> {
let status = self.mag_status()?;
if status.xyz_new_data {
let data = self
.iface
.read_mag_3_double_registers(Register::OUTX_L_REG_M)?;
Ok(UnscaledMeasurement {
x: data.0 as i16,
y: data.1 as i16,
z: data.2 as i16,
})
} else {
let cfg = self.iface.read_mag_register(Register::CFG_REG_A_M)?;
if (cfg & 0x3) != 0x1 {
let cfg = (self.cfg_reg_a_m.bits & 0xFC) | 0x1;
self.iface.write_mag_register(Register::CFG_REG_A_M, cfg)?;
}
Err(nb::Error::WouldBlock)
}
}
}
const SCALING_FACTOR: i32 = 150;
fn scale_measurement(unscaled: i16) -> i32 {
unscaled as i32 * SCALING_FACTOR
}