1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
use imu_sensors::{AccFullScale, AccUnits, GyroFullScale, GyroUnits, Imu};
use static_cell::StaticCell;
use vqm::Vector3f32;
use crate::boards::BoardImu;
/*#[cfg(any(feature = "rp2040", feature = "rp235xa", feature = "rp235xb"))]
use embassy_rp::{
gpio::{Input, Pull},
interrupt::{self, InterruptExt, Priority},
};*/
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ImuData {
pub acc: Vector3f32,
pub gyro_rps: Vector3f32,
pub delta_t: f32,
}
impl ImuData {
pub const fn new() -> Self {
Self {
acc: Vector3f32 { x: 0.0, y: 0.0, z: 0.0 },
gyro_rps: Vector3f32 { x: 0.0, y: 0.0, z: 0.0 },
delta_t: 0.1,
}
}
}
impl Default for ImuData {
fn default() -> Self {
Self::new()
}
}
pub static IMU_SIGNAL: Signal<CriticalSectionRawMutex, ImuData> = Signal::new();
static IMU_CTX: StaticCell<ImuContext<BoardImu>> = StaticCell::new();
pub fn init(imu: BoardImu) -> &'static mut ImuContext<BoardImu> {
IMU_CTX.init(ImuContext::<BoardImu>::new(imu))
}
/// IMU Task Placeholder.
#[embassy_executor::task]
pub async fn run(ctx: &'static mut ImuContext<BoardImu>) {
let delta_t_us = 1000;
let delta_t = 0.001_f32;
let mut ticker = embassy_time::Ticker::every(embassy_time::Duration::from_micros(delta_t_us));
let mut loop_count: u32 = 0;
_ = ctx.imu.init(8000, GyroFullScale::Max, GyroUnits::Rps, AccFullScale::Max, AccUnits::G).await;
log::info!(" IMU: task started");
loop {
// Wait for the next 50Hz tick.
ticker.next().await;
// For now we are just faking some gyro and acc values.
/*let acc_rnd = Vector3f32 { x: 1.0, y: 0.5, z: 0.25 };
ctx.imu.set_acc(acc_rnd).await;
x_base += rand.next_range(0..5_u32).cast_signed() - 2;
let gyro_x = x_base + rand.next_range(0..5_u32).cast_signed() - 2;
let gyro_y = rand.next_range(0..11_u32).cast_signed() - 5;
let gyro_z = rand.next_range(0..11_u32).cast_signed() - 5;
#[allow(clippy::cast_precision_loss)]
let gyro_dps_rnd = Vector3f32 { x: gyro_x as f32, y: gyro_y as f32, z: gyro_z as f32 };
ctx.imu.set_gyro(gyro_dps_rnd).await;*/
// ctx.drdy.wait_for_rising_edge().await; // Synchronized to IMU
// let data = read_imu_dma(&mut ctx.spi).await;
/*let (acc, gyro_rps) = match ctx.imu.read_acc_gyro_rps().await {
Ok(acc) => acc,
Err(e) => (Vector3f32::default(),Vector3f32::default()),
};*/
let acc_gyro = ctx.imu.read_acc_gyro().await;
let imu_data = match acc_gyro {
Ok(acc_gyro) => ImuData { acc: acc_gyro.0, gyro_rps: acc_gyro.1, delta_t },
Err(_acc_gyro) => ImuData { acc: Vector3f32::default(), gyro_rps: Vector3f32::default(), delta_t },
};
// Signal the gyro_pid task that there is new ImuData available.
//let imu_data = ImuData { acc: Vector3::default(), gyro_rps: Vector3::default(), delta_t };
IMU_SIGNAL.signal(imu_data);
if loop_count.is_multiple_of(1000) {
log::info!(" IMU: loop {loop_count}");
}
loop_count = loop_count.wrapping_add(1);
// Slow down the simulation for PC console
// 100ms is good for seeing the prints; change to 1ms for "real speed".
embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await;
}
}