cu_wt901/
lib.rs

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

cu_wt901/
lib.rs
