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(
147        &mut self,
148        ctx: &CuContext,
149        pr: &mut PositionalReadingsPayload,
150    ) -> Result<(), CuError> {
151        debug!(ctx, "Trying to read i2c");
152
153        #[cfg(hardware)]
154        {
155            let mut buf = [0u8; REGISTER_SPAN_SIZE];
156            self.i2c
157                .write_read(WT901_I2C_ADDRESS, &[Registers::AccX as u8], &mut buf)
158                .expect("Error reading WT901");
159            pr.acc_x = convert_acc(get_vec_i16(&buf, Registers::AccX.offset()));
160            pr.acc_y = convert_acc(get_vec_i16(&buf, Registers::AccY.offset()));
161            pr.acc_z = convert_acc(get_vec_i16(&buf, Registers::AccZ.offset()));
162            pr.gyro_x = convert_ang_vel(get_vec_i16(&buf, Registers::GyroX.offset()));
163            pr.gyro_y = convert_ang_vel(get_vec_i16(&buf, Registers::GyroY.offset()));
164            pr.gyro_z = convert_ang_vel(get_vec_i16(&buf, Registers::GyroZ.offset()));
165            pr.mag_x = convert_mag(get_vec_i16(&buf, Registers::MagX.offset()));
166            pr.mag_y = convert_mag(get_vec_i16(&buf, Registers::MagY.offset()));
167            pr.mag_z = convert_mag(get_vec_i16(&buf, Registers::MagZ.offset()));
168            pr.roll = convert_angle(get_vec_i16(&buf, Registers::Roll.offset()));
169            pr.pitch = convert_angle(get_vec_i16(&buf, Registers::Pitch.offset()));
170            pr.yaw = convert_angle(get_vec_i16(&buf, Registers::Yaw.offset()));
171        }
172        Ok(())
173    }
174}
175
176impl Freezable for WT901 {
177    // WT901 has no internal state, we can leave the default implementation.
178}
179
180impl CuSrcTask for WT901 {
181    type Resources<'r> = Wt901Resources;
182    type Output<'m> = output_msg!(PositionalReadingsPayload);
183
184    fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
185    where
186        Self: Sized,
187    {
188        let _ = _config;
189        #[cfg(hardware)]
190        let i2c = _resources.i2c.0;
191        Ok(WT901 {
192            #[cfg(hardware)]
193            i2c,
194        })
195    }
196
197    fn process(&mut self, ctx: &CuContext, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
198        let mut pos = PositionalReadingsPayload::default();
199        self.bulk_position_read(ctx, &mut pos)?;
200        new_msg.set_payload(pos);
201        Ok(())
202    }
203}
204
205/// Get a u16 value out of a u8 buffer
206#[inline]
207#[allow(dead_code)]
208fn get_vec_u16(buf: &[u8], offset: usize) -> u16 {
209    u16::from_le_bytes([buf[offset], buf[offset + 1]])
210}
211
212/// Get a u16 value out of a u8 buffer
213#[inline]
214#[allow(dead_code)]
215fn get_vec_i16(buf: &[u8], offset: usize) -> i16 {
216    i16::from_le_bytes([buf[offset], buf[offset + 1]])
217}
218
219#[allow(dead_code)]
220fn convert_acc(acc: i16) -> Acceleration {
221    // the scale is from 0 to 16g
222    let acc = acc as f32 / 32768.0 * 16.0;
223    Acceleration::new::<standard_gravity>(acc)
224}
225
226#[allow(dead_code)]
227fn convert_ang_vel(angv: i16) -> AngularVelocity {
228    // the scale is from 0 to 2000 deg/s
229    let acc = (angv as f32 / 32768.0) * 2000.0;
230    AngularVelocity::new::<degree_per_second>(acc)
231}
232
233#[allow(dead_code)]
234fn convert_mag(mag: i16) -> MagneticFluxDensity {
235    // the resolution is 8.333nT/LSB
236    let mag = (mag as f32 / 32768.0) * 8.333;
237    MagneticFluxDensity::new::<nanotesla>(mag)
238}
239
240#[allow(dead_code)]
241fn convert_angle(angle: i16) -> Angle {
242    let angle = angle as f32 / 32768.0 * 180.0;
243    Angle::new::<degree>(angle)
244}
cu_wt901/lib.rs

cu_wt901/
lib.rs
