cu_wt901/
lib.rs

1use bincode::de::Decoder;
2use bincode::enc::Encoder;
3use bincode::error::{DecodeError, EncodeError};
4use bincode::{Decode, Encode};
5use cu29::prelude::*;
6#[cfg(hardware)]
7use embedded_hal::i2c::I2c;
8#[cfg(hardware)]
9use linux_embedded_hal::{I2CError, I2cdev};
10use std::fmt::Display;
11use uom::si::acceleration::{meter_per_second_squared, standard_gravity};
12use uom::si::angle::{degree, radian};
13use uom::si::angular_velocity::{degree_per_second, radian_per_second};
14use uom::si::f32::Acceleration;
15use uom::si::f32::Angle;
16use uom::si::f32::AngularVelocity;
17use uom::si::f32::MagneticFluxDensity;
18use uom::si::magnetic_flux_density::{nanotesla, tesla};
19
20// FIXME: remove.
21const I2C_BUS: &str = "/dev/i2c-9";
22#[allow(unused)]
23const WT901_I2C_ADDRESS: u8 = 0x50;
24
25#[allow(unused)]
26#[repr(u8)]
27#[derive(Debug, Clone, Copy)]
28enum Registers {
29    // Accelerometer addresses
30    AccX = 0x34,
31    AccY = 0x35,
32    AccZ = 0x36,
33
34    // Gyroscope addresses
35    GyroX = 0x37,
36    GyroY = 0x38,
37    GyroZ = 0x39,
38
39    // Magnetometer addresses
40    MagX = 0x3A,
41    MagY = 0x3B,
42    MagZ = 0x3C,
43
44    // Orientation addresses
45    Roll = 0x3D,
46    Pitch = 0x3E,
47    Yaw = 0x3F,
48}
49
50impl Registers {
51    #[allow(dead_code)]
52    fn offset(&self) -> usize {
53        ((*self as u8 - Registers::AccX as u8) * 2) as usize
54    }
55}
56
57use cu29_log_derive::debug;
58use cu29_traits::CuError;
59use serde::de::{Deserialize, Deserializer};
60use serde::ser::{Serialize, SerializeStruct, Serializer};
61use uom::fmt::DisplayStyle::Abbreviation;
62
63pub struct WT901 {
64    #[cfg(hardware)]
65    i2c: Box<dyn I2c<Error = I2CError>>,
66}
67
68#[derive(Default, Clone, Debug)]
69pub struct PositionalReadingsPayload {
70    acc_x: Acceleration,
71    acc_y: Acceleration,
72    acc_z: Acceleration,
73    gyro_x: AngularVelocity,
74    gyro_y: AngularVelocity,
75    gyro_z: AngularVelocity,
76    mag_x: MagneticFluxDensity,
77    mag_y: MagneticFluxDensity,
78    mag_z: MagneticFluxDensity,
79    roll: Angle,
80    pitch: Angle,
81    yaw: Angle,
82}
83
84impl Display for PositionalReadingsPayload {
85    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
86        let acc_style = Acceleration::format_args(standard_gravity, Abbreviation);
87        let angv_style = AngularVelocity::format_args(degree_per_second, Abbreviation);
88        let mag_style = MagneticFluxDensity::format_args(nanotesla, Abbreviation);
89        let angle_style = Angle::format_args(degree, Abbreviation);
90
91        write!(
92            f,
93            "acc_x: {}, acc_y: {}, acc_z: {}\n gyro_x: {}, gyro_y: {}, gyro_z: {}\nmag_x: {}, mag_y: {}, mag_z: {}\nroll: {}, pitch: {}, yaw: {}",
94            acc_style.with(self.acc_x),
95            acc_style.with(self.acc_y),
96            acc_style.with(self.acc_z),
97            angv_style.with(self.gyro_x),
98            angv_style.with(self.gyro_y),
99            angv_style.with(self.gyro_z),
100            mag_style.with(self.mag_x),
101            mag_style.with(self.mag_y),
102            mag_style.with(self.mag_z),
103            angle_style.with(self.roll),
104            angle_style.with(self.pitch),
105            angle_style.with(self.yaw)
106        )
107    }
108}
109
110impl Serialize for PositionalReadingsPayload {
111    fn serialize<S: Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
112        let mut s = serializer.serialize_struct("PositionalReadings", 12)?;
113        s.serialize_field("acc_x", &self.acc_x.value)?;
114        s.serialize_field("acc_y", &self.acc_y.value)?;
115        s.serialize_field("acc_z", &self.acc_z.value)?;
116        s.serialize_field("gyro_x", &self.gyro_x.value)?;
117        s.serialize_field("gyro_y", &self.gyro_y.value)?;
118        s.serialize_field("gyro_z", &self.gyro_z.value)?;
119        s.serialize_field("mag_x", &self.mag_x.value)?;
120        s.serialize_field("mag_y", &self.mag_y.value)?;
121        s.serialize_field("mag_z", &self.mag_z.value)?;
122        s.serialize_field("roll", &self.roll.value)?;
123        s.serialize_field("pitch", &self.pitch.value)?;
124        s.serialize_field("yaw", &self.yaw.value)?;
125        s.end()
126    }
127}
128
129impl<'de> Deserialize<'de> for PositionalReadingsPayload {
130    fn deserialize<D: Deserializer<'de>>(deserializer: D) -> Result<Self, D::Error> {
131        let values = <[f32; 12]>::deserialize(deserializer)?;
132        Ok(PositionalReadingsPayload {
133            acc_x: Acceleration::new::<standard_gravity>(values[0]),
134            acc_y: Acceleration::new::<standard_gravity>(values[1]),
135            acc_z: Acceleration::new::<standard_gravity>(values[2]),
136            gyro_x: AngularVelocity::new::<degree_per_second>(values[3]),
137            gyro_y: AngularVelocity::new::<degree_per_second>(values[4]),
138            gyro_z: AngularVelocity::new::<degree_per_second>(values[5]),
139            mag_x: MagneticFluxDensity::new::<nanotesla>(values[6]),
140            mag_y: MagneticFluxDensity::new::<nanotesla>(values[7]),
141            mag_z: MagneticFluxDensity::new::<nanotesla>(values[8]),
142            roll: Angle::new::<degree>(values[9]),
143            pitch: Angle::new::<degree>(values[10]),
144            yaw: Angle::new::<degree>(values[11]),
145        })
146    }
147}
148
149impl Encode for PositionalReadingsPayload {
150    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
151        // Encode in natural SI units
152        Encode::encode(&self.acc_x.value, encoder)?;
153        bincode::Encode::encode(&self.acc_y.value, encoder)?;
154        bincode::Encode::encode(&self.acc_z.value, encoder)?;
155        bincode::Encode::encode(&self.gyro_x.value, encoder)?;
156        bincode::Encode::encode(&self.gyro_y.value, encoder)?;
157        bincode::Encode::encode(&self.gyro_z.value, encoder)?;
158        bincode::Encode::encode(&self.mag_x.value, encoder)?;
159        bincode::Encode::encode(&self.mag_y.value, encoder)?;
160        bincode::Encode::encode(&self.mag_z.value, encoder)?;
161        bincode::Encode::encode(&self.roll.value, encoder)?;
162        bincode::Encode::encode(&self.pitch.value, encoder)?;
163        bincode::Encode::encode(&self.yaw.value, encoder)?;
164        Ok(())
165    }
166}
167
168impl Decode<()> for PositionalReadingsPayload {
169    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
170        Ok(PositionalReadingsPayload {
171            // Decode back from the natural SI units
172            acc_x: Acceleration::new::<meter_per_second_squared>(bincode::Decode::decode(decoder)?),
173            acc_y: Acceleration::new::<meter_per_second_squared>(bincode::Decode::decode(decoder)?),
174            acc_z: Acceleration::new::<meter_per_second_squared>(bincode::Decode::decode(decoder)?),
175            gyro_x: AngularVelocity::new::<radian_per_second>(bincode::Decode::decode(decoder)?),
176            gyro_y: AngularVelocity::new::<radian_per_second>(bincode::Decode::decode(decoder)?),
177            gyro_z: AngularVelocity::new::<radian_per_second>(bincode::Decode::decode(decoder)?),
178            mag_x: MagneticFluxDensity::new::<tesla>(bincode::Decode::decode(decoder)?),
179            mag_y: MagneticFluxDensity::new::<tesla>(bincode::Decode::decode(decoder)?),
180            mag_z: MagneticFluxDensity::new::<tesla>(bincode::Decode::decode(decoder)?),
181            roll: Angle::new::<radian>(bincode::Decode::decode(decoder)?),
182            pitch: Angle::new::<radian>(bincode::Decode::decode(decoder)?),
183            yaw: Angle::new::<radian>(bincode::Decode::decode(decoder)?),
184        })
185    }
186}
187
188// Number of registers to read in one go
189#[allow(unused)]
190const REGISTER_SPAN_SIZE: usize = ((Registers::Yaw as u8 - Registers::AccX as u8) * 2 + 2) as usize;
191
192#[allow(unused)]
193impl WT901 {
194    fn bulk_position_read(&mut self, pr: &mut PositionalReadingsPayload) -> Result<(), CuError> {
195        debug!("Trying to read i2c");
196
197        #[cfg(hardware)]
198        {
199            let mut buf = [0u8; REGISTER_SPAN_SIZE];
200            self.i2c
201                .write_read(WT901_I2C_ADDRESS, &[Registers::AccX as u8], &mut buf)
202                .expect("Error reading WT901");
203            pr.acc_x = convert_acc(get_vec_i16(&buf, Registers::AccX.offset()));
204            pr.acc_y = convert_acc(get_vec_i16(&buf, Registers::AccY.offset()));
205            pr.acc_z = convert_acc(get_vec_i16(&buf, Registers::AccZ.offset()));
206            pr.gyro_x = convert_ang_vel(get_vec_i16(&buf, Registers::GyroX.offset()));
207            pr.gyro_y = convert_ang_vel(get_vec_i16(&buf, Registers::GyroY.offset()));
208            pr.gyro_z = convert_ang_vel(get_vec_i16(&buf, Registers::GyroZ.offset()));
209            pr.mag_x = convert_mag(get_vec_i16(&buf, Registers::MagX.offset()));
210            pr.mag_y = convert_mag(get_vec_i16(&buf, Registers::MagY.offset()));
211            pr.mag_z = convert_mag(get_vec_i16(&buf, Registers::MagZ.offset()));
212            pr.roll = convert_angle(get_vec_i16(&buf, Registers::Roll.offset()));
213            pr.pitch = convert_angle(get_vec_i16(&buf, Registers::Pitch.offset()));
214            pr.yaw = convert_angle(get_vec_i16(&buf, Registers::Yaw.offset()));
215        }
216        Ok(())
217    }
218}
219
220impl Freezable for WT901 {
221    // WT901 has no internal state, we can leave the default implementation.
222}
223
224impl CuSrcTask for WT901 {
225    type Resources<'r> = ();
226    type Output<'m> = output_msg!(PositionalReadingsPayload);
227
228    fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
229    where
230        Self: Sized,
231    {
232        debug!("Opening {}... ", I2C_BUS);
233        #[cfg(hardware)]
234        let i2cdev = I2cdev::new(I2C_BUS).unwrap();
235        debug!("{} opened.", I2C_BUS);
236        Ok(WT901 {
237            #[cfg(hardware)]
238            i2c: Box::new(i2cdev),
239        })
240    }
241
242    fn process(&mut self, _clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
243        let mut pos = PositionalReadingsPayload::default();
244        self.bulk_position_read(&mut pos)?;
245        new_msg.set_payload(pos);
246        Ok(())
247    }
248}
249
250/// Get a u16 value out of a u8 buffer
251#[inline]
252#[allow(dead_code)]
253fn get_vec_u16(buf: &[u8], offset: usize) -> u16 {
254    u16::from_le_bytes([buf[offset], buf[offset + 1]])
255}
256
257/// Get a u16 value out of a u8 buffer
258#[inline]
259#[allow(dead_code)]
260fn get_vec_i16(buf: &[u8], offset: usize) -> i16 {
261    i16::from_le_bytes([buf[offset], buf[offset + 1]])
262}
263
264#[allow(dead_code)]
265fn convert_acc(acc: i16) -> Acceleration {
266    // the scale is from 0 to 16g
267    let acc = acc as f32 / 32768.0 * 16.0;
268    Acceleration::new::<standard_gravity>(acc)
269}
270
271#[allow(dead_code)]
272fn convert_ang_vel(angv: i16) -> AngularVelocity {
273    // the scale is from 0 to 2000 deg/s
274    let acc = (angv as f32 / 32768.0) * 2000.0;
275    AngularVelocity::new::<degree_per_second>(acc)
276}
277
278#[allow(dead_code)]
279fn convert_mag(mag: i16) -> MagneticFluxDensity {
280    // the resolution is 8.333nT/LSB
281    let mag = (mag as f32 / 32768.0) * 8.333;
282    MagneticFluxDensity::new::<nanotesla>(mag)
283}
284
285#[allow(dead_code)]
286fn convert_angle(angle: i16) -> Angle {
287    let angle = angle as f32 / 32768.0 * 180.0;
288    Angle::new::<degree>(angle)
289}
cu_wt901/lib.rs

cu_wt901/
lib.rs
