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

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

use embassy_sync::{
    pubsub::{PubSubChannel, Publisher, Subscriber},
    {blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal},
};

use crate::{
    gps::{Geodetic, GeographicCoordinate, GpsSolutionData},
    gps::{
        GpsMessage, {GpsData, GpsPositionMeters, GpsYawHeadingMessage},
    },
};

const MAX_GPS_SUBSCRIBER_COUNT: usize = 4;
const GPS_PUBLISHER_COUNT: usize = 1;
const GPS_PUB_SUB_CAPACITY: usize = 4;

/// `PubSubChannel` for handling `GpsData` updates.
static GPS_PUB_SUB_CHANNEL: PubSubChannel<
    CriticalSectionRawMutex,
    GpsMessage,
    GPS_PUB_SUB_CAPACITY,
    MAX_GPS_SUBSCRIBER_COUNT,
    GPS_PUBLISHER_COUNT,
> = PubSubChannel::new();

pub type GpsPublisher = Publisher<
    'static,
    CriticalSectionRawMutex,
    GpsMessage,
    GPS_PUB_SUB_CAPACITY,
    MAX_GPS_SUBSCRIBER_COUNT,
    GPS_PUBLISHER_COUNT,
>;

#[allow(clippy::expect_used)]
pub fn gps_publisher() -> GpsPublisher {
    GPS_PUB_SUB_CHANNEL.publisher().expect("gps_publisher failed")
}

pub type GpsSubscriber = Subscriber<
    'static,
    CriticalSectionRawMutex,
    GpsMessage,
    GPS_PUB_SUB_CAPACITY,
    MAX_GPS_SUBSCRIBER_COUNT,
    GPS_PUBLISHER_COUNT,
>;

#[allow(clippy::expect_used)]
pub fn gps_subscriber() -> GpsSubscriber {
    GPS_PUB_SUB_CHANNEL.subscriber().expect("gps_subscriber failed")
}

pub static GPS_YAW_HEADING_SIGNAL: Signal<CriticalSectionRawMutex, GpsYawHeadingMessage> = Signal::new();

/// Context for GPS task.
pub struct GpsContext {
    pub gps_publisher: GpsPublisher,
    pub home: Geodetic,
}

impl GpsContext {
    pub const fn new(gps_publisher: GpsPublisher) -> Self {
        Self { gps_publisher, home: Geodetic::new() }
    }
}

/// GPS Task Placeholder.
#[embassy_executor::task]
pub async fn gps_task(ctx: &'static mut GpsContext) {
    let mut ticker = embassy_time::Ticker::every(embassy_time::Duration::from_hz(10));
    let mut loop_count: u32 = 0;

    log::info!("         GPS: task started");
    loop {
        // Wait for the next tick.
        ticker.next().await;

        // TODO: this should get the data from the actual GPS sensor.
        let gps_data = GpsData::default();
        let gps_solution = GpsSolutionData { satellite_count: 4, ..Default::default() };

        // Publish the raw gps data for use by (eg) the OSD.
        ctx.gps_publisher.publish_immediate(GpsMessage::Gps(gps_data));
        ctx.gps_publisher.publish_immediate(GpsMessage::GpsSolution(gps_solution));

        // Convert the gps_data position to a GpsPosition item (ie position in meters from home) for use by the autopilot.
        let geographic_coordinate = GeographicCoordinate::from(gps_data.position);
        let gps_position = GpsPositionMeters { position: ctx.home.distance_from_home_meters(geographic_coordinate) };
        ctx.gps_publisher.publish_immediate(GpsMessage::GpsPositionMeters(gps_position));

        // Only trust GPS heading if moving faster than 1.5 m/s (150 cmps, approx 3 knots)
        if gps_data.ground_speed_cmps > 150 {
            let gps_yaw_heading_message = GpsYawHeadingMessage {
                yaw_heading_radians: (f32::from(gps_data.heading_deci_degrees) * 0.1).to_radians(),
                delta_t: 0.1,
            };
            // signal the yaw heading so the gyro_pid task can use it to correct yaw drift in the sensor fusion filter.
            GPS_YAW_HEADING_SIGNAL.signal(gps_yaw_heading_message);
        }

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