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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#![no_std]
use embedded_hal::blocking::i2c::{Write, WriteRead};
pub mod cfg_reg_a;
pub mod cfg_reg_b;
pub mod cfg_reg_c;
use cfg_reg_a::CfgRegA;
use cfg_reg_b::CfgRegB;
use cfg_reg_c::CfgRegC;
pub const I2C_ADDRESS: u8 = 0x1eu8;
trait Register {
fn read<I2C>(&self, i2c: &mut I2C, reg_addr: u8) -> Result<u8, I2C::Error>
where
I2C: WriteRead,
{
let mut data: [u8; 1] = [0];
i2c.write_read(I2C_ADDRESS, &[reg_addr], &mut data)?;
Ok(data[0])
}
fn write<I2C>(&self, i2c: &mut I2C, reg_addr: u8, bits: u8) -> Result<(), I2C::Error>
where
I2C: Write,
{
i2c.write(I2C_ADDRESS, &[reg_addr, bits])
}
}
pub struct Iis2mdc {
pub cfg_reg_a: CfgRegA,
pub cfg_reg_b: CfgRegB,
pub cfg_reg_c: CfgRegC,
}
impl Iis2mdc {
pub fn new<I2C, E>(i2c: &mut I2C) -> Result<Iis2mdc, E>
where
I2C: WriteRead<Error = E> + Write<Error = E>,
{
let mut registers = [0u8; 3];
i2c.write_read(I2C_ADDRESS, &[0x60], &mut registers)?;
let cfg_reg_a = CfgRegA::new(registers[0]);
let cfg_reg_b = CfgRegB::new(registers[1]);
let cfg_reg_c = CfgRegC::new(registers[2]);
let iis2mdc = Iis2mdc {
cfg_reg_a,
cfg_reg_b,
cfg_reg_c,
};
Ok(iis2mdc)
}
pub fn get_temperature<I2C>(&mut self, i2c: &mut I2C) -> Result<f32, I2C::Error>
where
I2C: WriteRead,
{
let mut measurements = [0u8; 2];
i2c.write_read(I2C_ADDRESS, &[0x6e], &mut measurements)?;
let raw_temp = (measurements[1] as i16) << 8 | measurements[0] as i16;
let temp: f32 = (raw_temp as f32 / 256.0) + 25.0;
Ok(temp)
}
pub fn get_measurements<I2C>(&mut self, i2c: &mut I2C) -> Result<[f64; 3], I2C::Error>
where
I2C: WriteRead,
{
let mut measurements = [0u8; 6];
i2c.write_read(I2C_ADDRESS, &[0x68], &mut measurements)?;
let raw_mag_x = (measurements[1] as i16) << 8 | (measurements[0] as i16);
let raw_mag_y = (measurements[3] as i16) << 8 | (measurements[2] as i16);
let raw_mag_z = (measurements[5] as i16) << 8 | (measurements[4] as i16);
let mag_x = raw_mag_x as f64;
let mag_y = raw_mag_y as f64;
let mag_z = raw_mag_z as f64;
Ok([mag_x, mag_y, mag_z])
}
}