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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152

#![cfg(feature = "msp")]
#![allow(unused)]

//use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::PubSubChannel};
use stream_buf::{StreamBufReader, StreamBufWriter};

use crate::{
    config::{ConfigPublisher, FastConfigPublisher},
    multiwii_serial_protocol::{Msp, MspSensorData},
};

#[cfg(feature = "barometer")]
use crate::tasks::barometer_task::BarometerSubscriber;

#[cfg(feature = "battery")]
use crate::tasks::battery_task::BatterySubscriber;

#[cfg(feature = "gps")]
use crate::{gps::GpsMessage, tasks::gps_task::GpsSubscriber};

#[cfg(feature = "magnetometer")]
use crate::tasks::magnetometer_task::MagnetometerSubscriber;

#[cfg(feature = "optical_flow")]
use crate::tasks::optical_flow_task::OpticalFlowSubscriber;

#[cfg(feature = "rangefinder")]
use crate::tasks::rangefinder_task::RangefinderSubscriber;

/// Context for MSP task.
///
pub const MSP_READ_BUF_SIZE: usize = 256;
pub const MSP_WRITE_BUF_SIZE: usize = 512;
pub struct MspContext {
    pub fast_config_publisher: FastConfigPublisher,
    pub config_publisher: ConfigPublisher,
    #[cfg(feature = "barometer")]
    pub barometer_subscriber: BarometerSubscriber,
    #[cfg(feature = "battery")]
    pub battery_subscriber: BatterySubscriber,
    #[cfg(feature = "gps")]
    pub gps_subscriber: GpsSubscriber,
    #[cfg(feature = "magnetometer")]
    pub magnetometer_subscriber: MagnetometerSubscriber,
    #[cfg(feature = "optical_flow")]
    pub optical_flow_subscriber: OpticalFlowSubscriber,
    #[cfg(feature = "rangefinder")]
    pub rangefinder_subscriber: RangefinderSubscriber,
    pub msp: Msp,
    pub read_buf: [u8; MSP_READ_BUF_SIZE],
    pub write_buf: [u8; MSP_WRITE_BUF_SIZE],
}

impl MspContext {
    #[allow(clippy::too_many_arguments)]
    #[rustfmt::skip]
    pub fn new(
        fast_config_publisher: FastConfigPublisher,
        config_publisher: ConfigPublisher,
        #[cfg(feature = "barometer")] barometer_subscriber: BarometerSubscriber,
        #[cfg(feature = "battery")] battery_subscriber: BatterySubscriber,
        #[cfg(feature = "gps")] gps_subscriber: GpsSubscriber,
        #[cfg(feature = "magnetometer")] magnetometer_subscriber: MagnetometerSubscriber,
        #[cfg(feature = "optical_flow")] optical_flow_subscriber: OpticalFlowSubscriber,
        #[cfg(feature = "rangefinder")] rangefinder_subscriber: RangefinderSubscriber,
    ) -> Self {
        Self {
            msp: Msp::new(),
            fast_config_publisher,
            config_publisher,
            #[cfg(feature = "barometer")] barometer_subscriber,
            #[cfg(feature = "battery")] battery_subscriber,
            #[cfg(feature = "gps")] gps_subscriber,
            #[cfg(feature = "magnetometer")] magnetometer_subscriber,
            #[cfg(feature = "optical_flow")] optical_flow_subscriber,
            #[cfg(feature = "rangefinder")] rangefinder_subscriber,
            read_buf: [0u8; MSP_READ_BUF_SIZE],
            write_buf: [0u8; MSP_WRITE_BUF_SIZE],
        }
    }
}

impl MspContext {
    /// Helper to get a reader for `read_buf`.
    pub fn reader(&'_ mut self) -> StreamBufReader<'_> {
        StreamBufReader::new(&self.read_buf)
    }

    /// Helper to get a writer for `write_buf`.
    pub fn writer(&'_ mut self) -> StreamBufWriter<'_> {
        StreamBufWriter::new(&mut self.write_buf)
    }
}

/// MSP task Placeholder.
#[embassy_executor::task]
pub async fn msp_task(ctx: &'static mut MspContext) {
    // for now just wait on a ticker to drive the MSP loop. TODO: change this to wait on an MSP packet instead.
    let mut ticker = embassy_time::Ticker::every(embassy_time::Duration::from_millis(200));
    let mut loop_count: u32 = 0;

    // value to pass to Msp::process_write_command
    let mut msp_sensor_data = MspSensorData::new();

    log::info!("         MSP: task started");
    loop {
        // Wait for msp packet
        // let msp_packet = msp.receive().await;
        ticker.next().await; // for now just wait on ticker

        #[cfg(feature = "barometer")]
        #[allow(clippy::cast_possible_truncation)]
        if let Some(wait_result) = ctx.barometer_subscriber.try_next_message()
            && let embassy_sync::pubsub::WaitResult::Message(barometer_data) = wait_result
        {
            msp_sensor_data.barometer_altitude_cm = ((barometer_data.altitude_m * 100.0) as i32).cast_unsigned();
        }

        #[cfg(feature = "rangefinder")]
        #[allow(clippy::cast_possible_truncation)]
        if let Some(wait_result) = ctx.rangefinder_subscriber.try_next_message()
            && let embassy_sync::pubsub::WaitResult::Message(rangefinder_message) = wait_result
        {
            msp_sensor_data.rangefinder_altitude_cm = ((rangefinder_message.distance_m * 100.0) as i32).cast_unsigned();
        }

        #[cfg(feature = "gps")]
        if let Some(wait_result) = ctx.gps_subscriber.try_next_message()
            && let embassy_sync::pubsub::WaitResult::Message(event) = wait_result
            && let GpsMessage::GpsSolution(gps_solution_data) = event
        {
            msp_sensor_data.gps_sol.llh = gps_solution_data.llh;
            msp_sensor_data.gps_sol.satellite_count = gps_solution_data.satellite_count;
            msp_sensor_data.gps_sol.ground_speed_cmps = gps_solution_data.ground_speed_cmps;
            msp_sensor_data.gps_sol.ground_course_degrees_x10 = gps_solution_data.ground_course_degrees_x10;
            msp_sensor_data.gps_sol.dop_positional = gps_solution_data.dop.positional;
        }

        // Generally, we don't want to store the Reader itself because it tracks a "cursor" (current position).
        // It's better to store the data and create a fresh reader whenever we start processing a new packet.
        let mut src = StreamBufReader::new(&ctx.read_buf);

        let cmd_msp = Msp::SET_FAILSAFE_CONFIG;
        let _result =
            Msp::process_read_command(cmd_msp, &mut src, &ctx.config_publisher, &ctx.fast_config_publisher).await;

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