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), acc_style.with(self.acc_y), acc_style.with(self.acc_z),
95            angv_style.with(self.gyro_x), angv_style.with(self.gyro_y), angv_style.with(self.gyro_z),
96            mag_style.with(self.mag_x), mag_style.with(self.mag_y), mag_style.with(self.mag_z),
97            angle_style.with(self.roll), angle_style.with(self.pitch), angle_style.with(self.yaw)
98        )
99    }
100}
101
102impl Serialize for PositionalReadingsPayload {
103    fn serialize<S: Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
104        let mut s = serializer.serialize_struct("PositionalReadings", 12)?;
105        s.serialize_field("acc_x", &self.acc_x.value)?;
106        s.serialize_field("acc_y", &self.acc_y.value)?;
107        s.serialize_field("acc_z", &self.acc_z.value)?;
108        s.serialize_field("gyro_x", &self.gyro_x.value)?;
109        s.serialize_field("gyro_y", &self.gyro_y.value)?;
110        s.serialize_field("gyro_z", &self.gyro_z.value)?;
111        s.serialize_field("mag_x", &self.mag_x.value)?;
112        s.serialize_field("mag_y", &self.mag_y.value)?;
113        s.serialize_field("mag_z", &self.mag_z.value)?;
114        s.serialize_field("roll", &self.roll.value)?;
115        s.serialize_field("pitch", &self.pitch.value)?;
116        s.serialize_field("yaw", &self.yaw.value)?;
117        s.end()
118    }
119}
120
121impl<'de> Deserialize<'de> for PositionalReadingsPayload {
122    fn deserialize<D: Deserializer<'de>>(deserializer: D) -> Result<Self, D::Error> {
123        let values = <[f32; 12]>::deserialize(deserializer)?;
124        Ok(PositionalReadingsPayload {
125            acc_x: Acceleration::new::<standard_gravity>(values[0]),
126            acc_y: Acceleration::new::<standard_gravity>(values[1]),
127            acc_z: Acceleration::new::<standard_gravity>(values[2]),
128            gyro_x: AngularVelocity::new::<degree_per_second>(values[3]),
129            gyro_y: AngularVelocity::new::<degree_per_second>(values[4]),
130            gyro_z: AngularVelocity::new::<degree_per_second>(values[5]),
131            mag_x: MagneticFluxDensity::new::<nanotesla>(values[6]),
132            mag_y: MagneticFluxDensity::new::<nanotesla>(values[7]),
133            mag_z: MagneticFluxDensity::new::<nanotesla>(values[8]),
134            roll: Angle::new::<degree>(values[9]),
135            pitch: Angle::new::<degree>(values[10]),
136            yaw: Angle::new::<degree>(values[11]),
137        })
138    }
139}
140
141impl Encode for PositionalReadingsPayload {
142    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
143        // Encode in natural SI units
144        Encode::encode(&self.acc_x.value, encoder)?;
145        bincode::Encode::encode(&self.acc_y.value, encoder)?;
146        bincode::Encode::encode(&self.acc_z.value, encoder)?;
147        bincode::Encode::encode(&self.gyro_x.value, encoder)?;
148        bincode::Encode::encode(&self.gyro_y.value, encoder)?;
149        bincode::Encode::encode(&self.gyro_z.value, encoder)?;
150        bincode::Encode::encode(&self.mag_x.value, encoder)?;
151        bincode::Encode::encode(&self.mag_y.value, encoder)?;
152        bincode::Encode::encode(&self.mag_z.value, encoder)?;
153        bincode::Encode::encode(&self.roll.value, encoder)?;
154        bincode::Encode::encode(&self.pitch.value, encoder)?;
155        bincode::Encode::encode(&self.yaw.value, encoder)?;
156        Ok(())
157    }
158}
159
160impl Decode<()> for PositionalReadingsPayload {
161    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
162        Ok(PositionalReadingsPayload {
163            // Decode back from the natural SI units
164            acc_x: Acceleration::new::<meter_per_second_squared>(bincode::Decode::decode(decoder)?),
165            acc_y: Acceleration::new::<meter_per_second_squared>(bincode::Decode::decode(decoder)?),
166            acc_z: Acceleration::new::<meter_per_second_squared>(bincode::Decode::decode(decoder)?),
167            gyro_x: AngularVelocity::new::<radian_per_second>(bincode::Decode::decode(decoder)?),
168            gyro_y: AngularVelocity::new::<radian_per_second>(bincode::Decode::decode(decoder)?),
169            gyro_z: AngularVelocity::new::<radian_per_second>(bincode::Decode::decode(decoder)?),
170            mag_x: MagneticFluxDensity::new::<tesla>(bincode::Decode::decode(decoder)?),
171            mag_y: MagneticFluxDensity::new::<tesla>(bincode::Decode::decode(decoder)?),
172            mag_z: MagneticFluxDensity::new::<tesla>(bincode::Decode::decode(decoder)?),
173            roll: Angle::new::<radian>(bincode::Decode::decode(decoder)?),
174            pitch: Angle::new::<radian>(bincode::Decode::decode(decoder)?),
175            yaw: Angle::new::<radian>(bincode::Decode::decode(decoder)?),
176        })
177    }
178}
179
180// Number of registers to read in one go
181#[allow(unused)]
182const REGISTER_SPAN_SIZE: usize = ((Registers::Yaw as u8 - Registers::AccX as u8) * 2 + 2) as usize;
183
184#[allow(unused)]
185impl WT901 {
186    fn bulk_position_read(&mut self, pr: &mut PositionalReadingsPayload) -> Result<(), CuError> {
187        debug!("Trying to read i2c");
188
189        #[cfg(hardware)]
190        {
191            let mut buf = [0u8; REGISTER_SPAN_SIZE];
192            self.i2c
193                .write_read(WT901_I2C_ADDRESS, &[Registers::AccX as u8], &mut buf)
194                .expect("Error reading WT901");
195            pr.acc_x = convert_acc(get_vec_i16(&buf, Registers::AccX.offset()));
196            pr.acc_y = convert_acc(get_vec_i16(&buf, Registers::AccY.offset()));
197            pr.acc_z = convert_acc(get_vec_i16(&buf, Registers::AccZ.offset()));
198            pr.gyro_x = convert_ang_vel(get_vec_i16(&buf, Registers::GyroX.offset()));
199            pr.gyro_y = convert_ang_vel(get_vec_i16(&buf, Registers::GyroY.offset()));
200            pr.gyro_z = convert_ang_vel(get_vec_i16(&buf, Registers::GyroZ.offset()));
201            pr.mag_x = convert_mag(get_vec_i16(&buf, Registers::MagX.offset()));
202            pr.mag_y = convert_mag(get_vec_i16(&buf, Registers::MagY.offset()));
203            pr.mag_z = convert_mag(get_vec_i16(&buf, Registers::MagZ.offset()));
204            pr.roll = convert_angle(get_vec_i16(&buf, Registers::Roll.offset()));
205            pr.pitch = convert_angle(get_vec_i16(&buf, Registers::Pitch.offset()));
206            pr.yaw = convert_angle(get_vec_i16(&buf, Registers::Yaw.offset()));
207        }
208        Ok(())
209    }
210}
211
212impl Freezable for WT901 {
213    // WT901 has no internal state, we can leave the default implementation.
214}
215
216impl CuSrcTask for WT901 {
217    type Output<'m> = output_msg!(PositionalReadingsPayload);
218
219    fn new(_config: Option<&ComponentConfig>) -> CuResult<Self>
220    where
221        Self: Sized,
222    {
223        debug!("Opening {}... ", I2C_BUS);
224        #[cfg(hardware)]
225        let i2cdev = I2cdev::new(I2C_BUS).unwrap();
226        debug!("{} opened.", I2C_BUS);
227        Ok(WT901 {
228            #[cfg(hardware)]
229            i2c: Box::new(i2cdev),
230        })
231    }
232
233    fn process(&mut self, _clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
234        let mut pos = PositionalReadingsPayload::default();
235        self.bulk_position_read(&mut pos)?;
236        new_msg.set_payload(pos);
237        Ok(())
238    }
239}
240
241/// Get a u16 value out of a u8 buffer
242#[inline]
243#[allow(dead_code)]
244fn get_vec_u16(buf: &[u8], offset: usize) -> u16 {
245    u16::from_le_bytes([buf[offset], buf[offset + 1]])
246}
247
248/// Get a u16 value out of a u8 buffer
249#[inline]
250#[allow(dead_code)]
251fn get_vec_i16(buf: &[u8], offset: usize) -> i16 {
252    i16::from_le_bytes([buf[offset], buf[offset + 1]])
253}
254
255#[allow(dead_code)]
256fn convert_acc(acc: i16) -> Acceleration {
257    // the scale is from 0 to 16g
258    let acc = acc as f32 / 32768.0 * 16.0;
259    Acceleration::new::<standard_gravity>(acc)
260}
261
262#[allow(dead_code)]
263fn convert_ang_vel(angv: i16) -> AngularVelocity {
264    // the scale is from 0 to 2000 deg/s
265    let acc = (angv as f32 / 32768.0) * 2000.0;
266    AngularVelocity::new::<degree_per_second>(acc)
267}
268
269#[allow(dead_code)]
270fn convert_mag(mag: i16) -> MagneticFluxDensity {
271    // the resolution is 8.333nT/LSB
272    let mag = (mag as f32 / 32768.0) * 8.333;
273    MagneticFluxDensity::new::<nanotesla>(mag)
274}
275
276#[allow(dead_code)]
277fn convert_angle(angle: i16) -> Angle {
278    let angle = angle as f32 / 32768.0 * 180.0;
279    Angle::new::<degree>(angle)
280}
cu_wt901/lib.rs

cu_wt901/
lib.rs
