protoflight 0.1.2

Protoflight flight controller.
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
98
99

use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
use tinyrand::{RandRange, StdRand};

use imu_sensors::{AccFullScale, AccUnits, GyroFullScale, GyroUnits, ImuMock, MockImuBus};
use vqm::Vector3f32;

/*#[cfg(feature = "rp2350")]
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();

/// Context for IMU task.
pub struct ImuContext {
    pub imu: ImuMock<MockImuBus>,
}

impl ImuContext {
    pub fn new() -> Self {
        Self { imu: ImuMock::new(MockImuBus::new(), imu_sensors::ImuAxesOrder::XPOS_YPOS_ZPOS) }
    }
}

/// IMU Task Placeholder.
#[embassy_executor::task]
pub async fn imu_task(ctx: &'static mut ImuContext) {
    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;
    let mut rand = StdRand::default();
    // Base signal levels
    let mut x_base: i32 = 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_rps) = ctx.imu.read_acc_gyro_rps().await.unwrap_or_default();

        // Signal the gyro_pid task that there is new ImuData available.
        let imu_data = ImuData { acc: acc_rnd, gyro_rps: gyro_dps_rnd.to_radians(), delta_t };
        IMU_SIGNAL.signal(imu_data);

        if loop_count.is_multiple_of(100) {
            log::info!("           IMU:      loop {loop_count}");
        }
        loop_count = loop_count.wrapping_add(1); // use wrapping_add to handle when time rolls over at max u32.

        // 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;
    }
}