cu_wt901/
lib.rs

1use bincode::{Decode, Encode};
2#[cfg(hardware)]
3use cu_linux_resources::LinuxI2c;
4use cu29::prelude::*;
5#[cfg(hardware)]
6use cu29::resource::Owned;
7use cu29::resource::{ResourceBindingMap, ResourceBindings, ResourceManager};
8use cu29::units::si::acceleration::standard_gravity;
9use cu29::units::si::angle::degree;
10use cu29::units::si::angular_velocity::degree_per_second;
11use cu29::units::si::f32::{Acceleration, Angle, AngularVelocity, MagneticFluxDensity};
12use cu29::units::si::magnetic_flux_density::nanotesla;
13#[cfg(hardware)]
14use embedded_hal::i2c::I2c;
15use std::fmt::Display;
16
17#[allow(unused)]
18const WT901_I2C_ADDRESS: u8 = 0x50;
19
20#[allow(unused)]
21#[repr(u8)]
22#[derive(Debug, Clone, Copy)]
23enum Registers {
24    // Accelerometer addresses
25    AccX = 0x34,
26    AccY = 0x35,
27    AccZ = 0x36,
28
29    // Gyroscope addresses
30    GyroX = 0x37,
31    GyroY = 0x38,
32    GyroZ = 0x39,
33
34    // Magnetometer addresses
35    MagX = 0x3A,
36    MagY = 0x3B,
37    MagZ = 0x3C,
38
39    // Orientation addresses
40    Roll = 0x3D,
41    Pitch = 0x3E,
42    Yaw = 0x3F,
43}
44
45impl Registers {
46    #[allow(dead_code)]
47    fn offset(&self) -> usize {
48        ((*self as u8 - Registers::AccX as u8) * 2) as usize
49    }
50}
51
52use cu29_log_derive::debug;
53use cu29_traits::CuError;
54use serde::{Deserialize, Serialize};
55
56#[derive(Reflect)]
57#[reflect(from_reflect = false)]
58pub struct WT901 {
59    #[cfg(hardware)]
60    #[reflect(ignore)]
61    i2c: LinuxI2c,
62}
63
64#[derive(Copy, Clone, Debug, Eq, PartialEq)]
65pub enum Binding {
66    I2c,
67}
68
69pub struct Wt901Resources {
70    #[cfg(hardware)]
71    pub i2c: Owned<LinuxI2c>,
72}
73
74impl<'r> ResourceBindings<'r> for Wt901Resources {
75    type Binding = Binding;
76
77    fn from_bindings(
78        manager: &'r mut ResourceManager,
79        mapping: Option<&ResourceBindingMap<Self::Binding>>,
80    ) -> CuResult<Self> {
81        #[cfg(hardware)]
82        {
83            let mapping = mapping.ok_or_else(|| {
84                CuError::from("WT901 requires an `i2c` resource mapping in copperconfig")
85            })?;
86            let path = mapping.get(Binding::I2c).ok_or_else(|| {
87                CuError::from("WT901 resources must include `i2c: <bundle.resource>`")
88            })?;
89            let i2c = manager
90                .take::<LinuxI2c>(path.typed())
91                .map_err(|e| e.add_cause("Failed to fetch WT901 I2C resource"))?;
92            Ok(Self { i2c })
93        }
94        #[cfg(mock)]
95        {
96            let _ = manager;
97            let _ = mapping;
98            Ok(Self {})
99        }
100    }
101}
102
103#[derive(Default, Clone, Debug, Serialize, Deserialize, Encode, Decode, Reflect)]
104pub struct PositionalReadingsPayload {
105    acc_x: Acceleration,
106    acc_y: Acceleration,
107    acc_z: Acceleration,
108    gyro_x: AngularVelocity,
109    gyro_y: AngularVelocity,
110    gyro_z: AngularVelocity,
111    mag_x: MagneticFluxDensity,
112    mag_y: MagneticFluxDensity,
113    mag_z: MagneticFluxDensity,
114    roll: Angle,
115    pitch: Angle,
116    yaw: Angle,
117}
118
119impl Display for PositionalReadingsPayload {
120    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
121        write!(
122            f,
123            "acc_x: {} g, acc_y: {} g, acc_z: {} g\n gyro_x: {} deg/s, gyro_y: {} deg/s, gyro_z: {} deg/s\nmag_x: {} nT, mag_y: {} nT, mag_z: {} nT\nroll: {} deg, pitch: {} deg, yaw: {} deg",
124            self.acc_x.get::<standard_gravity>(),
125            self.acc_y.get::<standard_gravity>(),
126            self.acc_z.get::<standard_gravity>(),
127            self.gyro_x.get::<degree_per_second>(),
128            self.gyro_y.get::<degree_per_second>(),
129            self.gyro_z.get::<degree_per_second>(),
130            self.mag_x.get::<nanotesla>(),
131            self.mag_y.get::<nanotesla>(),
132            self.mag_z.get::<nanotesla>(),
133            self.roll.get::<degree>(),
134            self.pitch.get::<degree>(),
135            self.yaw.get::<degree>()
136        )
137    }
138}
139
140// Number of registers to read in one go
141#[allow(unused)]
142const REGISTER_SPAN_SIZE: usize = ((Registers::Yaw as u8 - Registers::AccX as u8) * 2 + 2) as usize;
143
144#[allow(unused)]
145impl WT901 {
146    fn bulk_position_read(&mut self, pr: &mut PositionalReadingsPayload) -> Result<(), CuError> {
147        debug!("Trying to read i2c");
148
149        #[cfg(hardware)]
150        {
151            let mut buf = [0u8; REGISTER_SPAN_SIZE];
152            self.i2c
153                .write_read(WT901_I2C_ADDRESS, &[Registers::AccX as u8], &mut buf)
154                .expect("Error reading WT901");
155            pr.acc_x = convert_acc(get_vec_i16(&buf, Registers::AccX.offset()));
156            pr.acc_y = convert_acc(get_vec_i16(&buf, Registers::AccY.offset()));
157            pr.acc_z = convert_acc(get_vec_i16(&buf, Registers::AccZ.offset()));
158            pr.gyro_x = convert_ang_vel(get_vec_i16(&buf, Registers::GyroX.offset()));
159            pr.gyro_y = convert_ang_vel(get_vec_i16(&buf, Registers::GyroY.offset()));
160            pr.gyro_z = convert_ang_vel(get_vec_i16(&buf, Registers::GyroZ.offset()));
161            pr.mag_x = convert_mag(get_vec_i16(&buf, Registers::MagX.offset()));
162            pr.mag_y = convert_mag(get_vec_i16(&buf, Registers::MagY.offset()));
163            pr.mag_z = convert_mag(get_vec_i16(&buf, Registers::MagZ.offset()));
164            pr.roll = convert_angle(get_vec_i16(&buf, Registers::Roll.offset()));
165            pr.pitch = convert_angle(get_vec_i16(&buf, Registers::Pitch.offset()));
166            pr.yaw = convert_angle(get_vec_i16(&buf, Registers::Yaw.offset()));
167        }
168        Ok(())
169    }
170}
171
172impl Freezable for WT901 {
173    // WT901 has no internal state, we can leave the default implementation.
174}
175
176impl CuSrcTask for WT901 {
177    type Resources<'r> = Wt901Resources;
178    type Output<'m> = output_msg!(PositionalReadingsPayload);
179
180    fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
181    where
182        Self: Sized,
183    {
184        let _ = _config;
185        #[cfg(hardware)]
186        let i2c = _resources.i2c.0;
187        Ok(WT901 {
188            #[cfg(hardware)]
189            i2c,
190        })
191    }
192
193    fn process(&mut self, _clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
194        let mut pos = PositionalReadingsPayload::default();
195        self.bulk_position_read(&mut pos)?;
196        new_msg.set_payload(pos);
197        Ok(())
198    }
199}
200
201/// Get a u16 value out of a u8 buffer
202#[inline]
203#[allow(dead_code)]
204fn get_vec_u16(buf: &[u8], offset: usize) -> u16 {
205    u16::from_le_bytes([buf[offset], buf[offset + 1]])
206}
207
208/// Get a u16 value out of a u8 buffer
209#[inline]
210#[allow(dead_code)]
211fn get_vec_i16(buf: &[u8], offset: usize) -> i16 {
212    i16::from_le_bytes([buf[offset], buf[offset + 1]])
213}
214
215#[allow(dead_code)]
216fn convert_acc(acc: i16) -> Acceleration {
217    // the scale is from 0 to 16g
218    let acc = acc as f32 / 32768.0 * 16.0;
219    Acceleration::new::<standard_gravity>(acc)
220}
221
222#[allow(dead_code)]
223fn convert_ang_vel(angv: i16) -> AngularVelocity {
224    // the scale is from 0 to 2000 deg/s
225    let acc = (angv as f32 / 32768.0) * 2000.0;
226    AngularVelocity::new::<degree_per_second>(acc)
227}
228
229#[allow(dead_code)]
230fn convert_mag(mag: i16) -> MagneticFluxDensity {
231    // the resolution is 8.333nT/LSB
232    let mag = (mag as f32 / 32768.0) * 8.333;
233    MagneticFluxDensity::new::<nanotesla>(mag)
234}
235
236#[allow(dead_code)]
237fn convert_angle(angle: i16) -> Angle {
238    let angle = angle as f32 / 32768.0 * 180.0;
239    Angle::new::<degree>(angle)
240}
cu_wt901/lib.rs

cu_wt901/
lib.rs
