protoflight 0.1.4

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

#![cfg(feature = "magnetometer")]

use embassy_sync::{
    blocking_mutex::raw::CriticalSectionRawMutex,
    pubsub::{PubSubChannel, Publisher, Subscriber},
};
use static_cell::StaticCell;

use crate::magnetometer_sensors::{Magnetometer, MagnetometerMessage, RxMagnetometer};

static MAGNETOMETER_CTX: StaticCell<MagnetometerContext> = StaticCell::new();

const MAX_MAGNETOMETER_SUBSCRIBER_COUNT: usize = 4;
const MAGNETOMETER_PUBLISHER_COUNT: usize = 1;
const MAGNETOMETER_PUB_SUB_CAPACITY: usize = 1; // only keep the last item

/// `PubSubChannel` for handling `Magnetometer` updates.
static MAGNETOMETER_PUB_SUB_CHANNEL: PubSubChannel<
    CriticalSectionRawMutex,
    MagnetometerMessage,
    MAGNETOMETER_PUB_SUB_CAPACITY,
    MAX_MAGNETOMETER_SUBSCRIBER_COUNT,
    MAGNETOMETER_PUBLISHER_COUNT,
> = PubSubChannel::new();

type MagnetometerPublisher = Publisher<
    'static,
    CriticalSectionRawMutex,
    MagnetometerMessage,
    MAGNETOMETER_PUB_SUB_CAPACITY,
    MAX_MAGNETOMETER_SUBSCRIBER_COUNT,
    MAGNETOMETER_PUBLISHER_COUNT,
>;

pub type MagnetometerSubscriber = Subscriber<
    'static,
    CriticalSectionRawMutex,
    MagnetometerMessage,
    MAGNETOMETER_PUB_SUB_CAPACITY,
    MAX_MAGNETOMETER_SUBSCRIBER_COUNT,
    MAGNETOMETER_PUBLISHER_COUNT,
>;

#[allow(unused, clippy::expect_used)]
pub fn magnetometer_subscriber() -> MagnetometerSubscriber {
    MAGNETOMETER_PUB_SUB_CHANNEL.subscriber().expect("magnetometer_subscriber failed")
}

/// Context for Magnetometer task.
pub struct MagnetometerContext {
    pub magnetometer: Magnetometer,
    pub magnetometer_publisher: MagnetometerPublisher,
}

pub fn init(magnetometer: Magnetometer) -> &'static mut MagnetometerContext {
    let ctx = MagnetometerContext {
        magnetometer,
        #[allow(clippy::expect_used)]
        magnetometer_publisher: MAGNETOMETER_PUB_SUB_CHANNEL.publisher().expect("magnetometer_publisher failed"),
    };
    MAGNETOMETER_CTX.init(ctx)
}

/// Magnetometer Task Placeholder.
#[embassy_executor::task]
pub async fn run(ctx: &'static mut MagnetometerContext) {
    const TASK_FREQUENCY_HZ: u64 = 40;
    let mut ticker = embassy_time::Ticker::every(embassy_time::Duration::from_hz(TASK_FREQUENCY_HZ));
    let mut loop_count: u32 = 0;

    log::info!("MAGNETOMETER: task started");
    loop {
        // Wait for the next tick.
        ticker.next().await;
        let magnetometer_message = ctx.magnetometer.message();
        // Publish a message, but if the queue is full, just kick out the oldest message.
        // This may cause some subscribers to miss a message
        ctx.magnetometer_publisher.publish_immediate(magnetometer_message);

        if loop_count.is_multiple_of(10) {
            log::info!("    MAG:      loop {loop_count}");
        }
        loop_count = loop_count.wrapping_add(1);
    }
}