r_d_inclita_sofware/sensors/
bno055.rs

1use crate::telemetry::data::{DATA_CHANNEL, ImuData, LATEST_TELEMETRY, LogEvent};
2use bno055::{BNO055OperationMode, Bno055};
3use defmt::{error, info};
4use embassy_stm32::i2c::I2c;
5use embassy_stm32::mode::Blocking;
6use embassy_time::{Delay, Duration, Instant, Ticker, Timer};
7
8#[embassy_executor::task]
9pub async fn bno055_logger_task(i2c_bus: I2c<'static, Blocking, embassy_stm32::i2c::Master>) {
10    info!("Starting BNO055 IMU Task...");
11
12    // The BNO055 crate needs a standard embedded-hal Delay to wait during bootup
13    let mut delay = Delay;
14    let mut imu = Bno055::new(i2c_bus);
15    let mut ticker = Ticker::every(Duration::from_millis(100)); //ticker that fires at a 100Hz freq
16
17    //Boot up the sensor
18    if let Err(e) = imu.init(&mut delay) {
19        error!("Failed to initialize BNO055! {:?}", e);
20        loop {
21            Timer::after_secs(1).await;
22        }
23    }
24
25    //Set to NDOF mode (Sensor Fusion ON)
26    if let Err(e) = imu.set_mode(BNO055OperationMode::NDOF, &mut delay) {
27        error!("Failed to set BNO055 mode! {:?}", e);
28        loop {
29            Timer::after_secs(1).await;
30        }
31    }
32
33    info!("BNO055 initialized and fused! Starting data loop (100Hz)...");
34
35    let mut timestamp_ms;
36
37    loop {
38        timestamp_ms = Instant::now().as_millis() as u32;
39
40        // (If any read fails, we just use 0.0 or the last known value to keep logging alive
41        let (yaw, pitch, roll) = match imu.euler_angles() {
42            Ok(euler) => (euler.c, euler.a, euler.b), //TODO:check the AXIS
43            Err(_) => (0.0, 0.0, 0.0),
44        };
45        let lin_accel = imu.linear_acceleration().unwrap_or(bno055::mint::Vector3 {
46            x: 0.0,
47            y: 0.0,
48            z: 0.0,
49        });
50        let gyro = imu.gyro_data().unwrap_or(bno055::mint::Vector3 {
51            x: 0.0,
52            y: 0.0,
53            z: 0.0,
54        });
55        let mag = imu.mag_data().unwrap_or(bno055::mint::Vector3 {
56            x: 0.0,
57            y: 0.0,
58            z: 0.0,
59        });
60        // this is kinda of a rabbithole / overkill if you dont make computations with this data, but for supporting control algorithms id recommend you see
61        // https://crates.io/crates/uom
62
63        let data = ImuData {
64            yaw,
65            pitch,
66            roll,
67
68            mag_x: mag.x,
69            mag_y: mag.y,
70            mag_z: mag.z,
71
72            gyro_x: gyro.x,
73            gyro_y: gyro.y,
74            gyro_z: gyro.z,
75
76            lin_accel_n: lin_accel.x,
77            lin_accel_e: lin_accel.y,
78            lin_accel_d: lin_accel.z,
79
80            timestamp_ms,
81        };
82        info!("{:?}", data);
83        // Send to the SD Card
84        DATA_CHANNEL.send(LogEvent::Imu(data.clone())).await;
85        {
86            let mut guard = LATEST_TELEMETRY.lock().await;
87
88            guard.imu = Some(data.into());
89        }
90        // Yield back to embassy
91        ticker.next().await;
92    }
93}
r_d_inclita_sofware/sensors/bno055.rs

r_d_inclita_sofware/sensors/
bno055.rs
