protoflight 0.1.4

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

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

use static_cell::StaticCell;

use crate::{
    boards::{GpsUartRx, GpsUartTx},
    config::GLOBAL_CONFIG,
    gps::{
        Geodetic, GeographicCoordinate, GpsMessage, GpsParser, GpsParserEvent, GpsProvider, GpsSolution, GpsStatus,
        GpsYawHeadingMessage,
        nmea::{NmeaGga, NmeaGsa, NmeaRecordType, NmeaRmc},
        ubx::{
            UbxAckId, UbxCfgId, UbxCfgNav5, UbxCfgPms, UbxCfgRate, UbxClassId, UbxMonId, UbxNavDop, UbxNavId, UbxNavPvt,
        },
    },
};

static GPS_CTX: StaticCell<GpsContext> = StaticCell::new();

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();

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

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.
#[allow(unused)]
pub struct GpsContext {
    pub uart_rx: GpsUartRx,
    pub uart_tx: GpsUartTx,
    pub gps_parser: GpsParser,
    pub gps_publisher: GpsPublisher,
    pub gps_data: GpsSolution,
    pub gps_status_data: GpsStatus,
    pub home: Geodetic,
    pub buf: [u8; Self::BUF_SIZE],
}

impl GpsContext {
    const BUF_SIZE: usize = 128;
}

#[allow(unused)]
pub fn init(uart_rx: GpsUartRx, uart_tx: GpsUartTx, gps_provider: GpsProvider) -> &'static mut GpsContext {
    let ctx = GpsContext {
        uart_rx,
        uart_tx,
        gps_parser: GpsParser::new_unwrapped(gps_provider),
        #[allow(clippy::expect_used)]
        gps_publisher: GPS_PUB_SUB_CHANNEL.publisher().expect("gps_publisher failed"),
        gps_data: GpsSolution::new(),
        gps_status_data: GpsStatus::new(),
        home: Geodetic::new(),
        buf: [0u8; GpsContext::BUF_SIZE],
    };
    GPS_CTX.init(ctx)
}

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

    log::info!("         GPS: task started");

    initialize(&mut ctx.uart_tx).await;

    loop {
        // Wait for the next tick. TODO: chang GPS task to run off interrupt
        ticker.next().await;
        core::future::poll_fn(|_| core::task::Poll::Ready(())).await;

        if let Ok(n) = ctx.read_packet().await {
            for &byte in &ctx.buf[..n] {
                if let Some(event) = ctx.gps_parser.on_data_received(byte) {
                    process_gps_event(&mut ctx.gps_data, &mut ctx.gps_status_data, event);

                    // Publish the gps data for use by (eg) the OSD.
                    ctx.gps_publisher.publish_immediate(GpsMessage::Data(ctx.gps_data));
                    ctx.gps_publisher.publish_immediate(GpsMessage::Status(ctx.gps_status_data));

                    // Convert the gps data position to a `GeographicCoordinate` for use by the autopilot.
                    let geographic_coordinate = GeographicCoordinate::from_long_lat_alt(
                        ctx.gps_data.longitude_degrees_x1e7,
                        ctx.gps_data.latitude_degrees_x1e7,
                        ctx.gps_data.altitude_cm,
                    );
                    let gps_position = ctx.home.position_relative_to_home_meters(geographic_coordinate);
                    ctx.gps_publisher.publish_immediate(GpsMessage::Position(gps_position));

                    // Only trust GPS heading if moving faster than 1.5 m/s (150 cmps, approx 3 knots)
                    if ctx.gps_data.ground_speed_cmps > 150 {
                        let gps_yaw_heading_message = GpsYawHeadingMessage {
                            yaw_heading_radians: (f32::from(ctx.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);
    }
}

async fn initialize(uart_tx: &mut GpsUartTx) {
    let global_config = GLOBAL_CONFIG.lock().await;
    let gps_config = global_config.gps;
    drop(global_config);

    // TODO: initialize GPS before main task loop.
    #[cfg(not(feature = "host"))]
    {
        let nav5 = UbxCfgNav5::new(gps_config.gps_ublox_acquire_model as u8);
        _ = uart_tx.write(&nav5.make_frame()).await;
        let pms = UbxCfgPms::default();
        _ = uart_tx.write(&pms.make_frame()).await;
        let rate = UbxCfgRate::default();
        _ = uart_tx.write(&rate.make_frame()).await;
    }
    #[cfg(feature = "host")]
    {
        // Use up the unused items.
        let &mut () = uart_tx;
        _ = gps_config;
        _ = UbxCfgNav5::default();
        _ = UbxCfgPms::default();
        _ = UbxCfgRate::default();
    }
}

fn process_gps_event(gps_data: &mut GpsSolution, gps_status: &mut GpsStatus, event: GpsParserEvent<'_>) {
    match event {
        GpsParserEvent::NmeaComplete(record) => match NmeaRecordType::from_record(record) {
            Some(NmeaRecordType::Gga) => {
                if let Some(gga) = NmeaGga::parse(record) {
                    gps_data.amend_with_nmea_gga(gga);
                    gps_status.amend_with_nmea_gga(gga);
                }
            }
            Some(NmeaRecordType::Rmc) => {
                if let Some(rmc) = NmeaRmc::parse(record) {
                    gps_data.amend_with_nmea_rmc(rmc);
                    gps_status.amend_with_nmea_rmc(rmc);
                }
            }
            Some(NmeaRecordType::Gsa) => {
                if let Some(gsa) = NmeaGsa::parse(record) {
                    gps_data.amend_with_nmea_gsa(gsa);
                    gps_status.amend_with_nmea_gsa(gsa);
                }
            }
            None => {}
        },

        GpsParserEvent::UbxMessage(message) => match message.class {
            UbxClassId::Nav => match message.id {
                UbxNavId::PVT => {
                    if let Some(nav_pvt) = UbxNavPvt::parse(message.payload) {
                        gps_data.amend_with_ubx_nav_pvt(nav_pvt);
                        gps_status.amend_with_ubx_nav_pvt(nav_pvt);
                    }
                }
                UbxNavId::DOP => {
                    if let Some(nav_dop) = UbxNavDop::parse(message.payload) {
                        gps_data.amend_with_ubx_nav_dop(nav_dop);
                        gps_status.amend_with_ubx_nav_dop(nav_dop);
                    }
                }
                _ => {}
            },

            #[allow(clippy::match_same_arms)]
            UbxClassId::Ack => match message.id {
                UbxAckId::ACK => {}
                UbxAckId::NAK => {}
                _ => {}
            },
            UbxClassId::Mon => if message.id == UbxMonId::VER {},

            #[allow(clippy::match_same_arms)]
            UbxClassId::Cfg => match message.id {
                UbxCfgId::PRT => {}
                UbxCfgId::MSG => {}
                UbxCfgId::NAV5 => {}
                UbxCfgId::PMS => {}
                UbxCfgId::SBAS => {}
                _ => {}
            },
            _ => {}
        },
    }
}

impl GpsContext {
    /// Read data from the UART with line-idle/break detection in a target-agnostic way.
    pub async fn read_packet(&mut self) -> Result<usize, ()> {
        #[cfg(any(feature = "rp2040", feature = "rp235xa", feature = "rp235xb"))]
        {
            // embassy_rp returns a Result<usize, ReadToBreakError>
            match self.uart_rx.read_to_break(&mut self.buf).await {
                Ok(n) => Ok(n),
                Err(_) => Err(()), // Map platform errors to a simple generic error
            }
        }

        #[cfg(feature = "stm32")]
        {
            // embassy_stm32 returns a Result<usize, Error> via idle line detection
            match self.uart_rx.read_until_idle(&mut self.buf).await {
                Ok(n) => Ok(n),
                Err(_) => Err(()),
            }
        }
        #[cfg(feature = "host")]
        {
            core::future::ready(()).await;
            Err(())
        }
    }
}

#[cfg(test)]
mod tests {
    #![allow(clippy::expect_used)]
    use crate::gps::{UbxMessage, make_realistic_nav_pvt_payload};

    use super::*;

    #[test]
    fn process_gps_event_amends_gga() {
        let record = b"GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,";

        let mut gps = GpsSolution::default();
        let mut gps_status = GpsStatus::default();

        process_gps_event(&mut gps, &mut gps_status, GpsParserEvent::NmeaComplete(record));

        //assert_eq!(gps.time_of_week_ms, 45_519_000);
        assert_eq!(gps.latitude_degrees_x1e7, 481_173_000);
        assert_eq!(gps.longitude_degrees_x1e7, 115_166_667);
        assert_eq!(gps.altitude_cm, 54_540);
        assert_eq!(gps.geoid_separation_cm, 4_690);
        assert_eq!(gps.fix, 1);
        assert_eq!(gps.satellite_count, 8);
    }
    #[test]
    fn process_gps_event_amends_nav_pvt() {
        let payload = make_realistic_nav_pvt_payload();

        let mut gps = GpsSolution::default();
        let mut gps_status = GpsStatus::default();

        let message = UbxMessage { class: UbxClassId::Nav, id: 0x07, payload: &payload };

        process_gps_event(&mut gps, &mut gps_status, GpsParserEvent::UbxMessage(message));

        assert_eq!(gps.time_of_week_ms, 45_296_789);
        assert_eq!(gps.longitude_degrees_x1e7, -12_345_678);
        assert_eq!(gps.latitude_degrees_x1e7, 512_345_678);
        assert_eq!(gps.altitude_cm, 10_000);
        assert_eq!(gps.geoid_separation_cm, 2345);
        assert_eq!(gps.velocity_north_cmps, 123);
        assert_eq!(gps.velocity_east_cmps, -45);
        assert_eq!(gps.velocity_down_cmps, 12);
        assert_eq!(gps.ground_speed_cmps, 131);
        assert_eq!(gps.heading_deci_degrees, 1_234);
        assert_eq!(gps.satellite_count, 12);
        assert_eq!(gps.fix, 3);
        assert_eq!(gps.is_healthy, 1);
    }
    #[test]
    fn process_gps_event_amends_rmc() {
        let record = b"GPRMC,..."; // existing known-good RMC fixture

        let mut gps = GpsSolution::default();
        let mut gps_status = GpsStatus::default();

        process_gps_event(&mut gps, &mut gps_status, GpsParserEvent::NmeaComplete(record));

        /*assert_eq!(gps.time_of_day_ms, /* expected */);
        assert_eq!(gps.latitude_degrees_x1e7, /* expected */);
        assert_eq!(gps.longitude_degrees_x1e7, /* expected */);
        assert_eq!(gps.ground_speed_cmps, /* expected */);
        assert_eq!(gps.heading_deci_degrees, /* expected */);*/
    }
    #[test]
    fn process_gps_event_amends_gsa() {
        let record = b"GPGSA,..."; // existing known-good GSA fixture

        let mut gps = GpsSolution::default();
        let mut gps_status = GpsStatus::default();

        process_gps_event(&mut gps, &mut gps_status, GpsParserEvent::NmeaComplete(record));

        /*assert_eq!(
            gps.dilution_of_precision_positional,
            /* expected */
        );*/
    }
    #[test]
    fn process_gps_event_ignores_unknown_nmea_record() {
        let mut gps = GpsSolution::default();
        let mut gps_status = GpsStatus::default();

        process_gps_event(&mut gps, &mut gps_status, GpsParserEvent::NmeaComplete(b"GPXXX,something"));

        assert_eq!(gps, GpsSolution::default());
    }
    #[test]
    fn process_gps_event_ignores_unknown_ubx_message() {
        let mut gps = GpsSolution::default();
        let mut gps_status = GpsStatus::default();

        let message = UbxMessage { class: UbxClassId::Nav, id: 0x99, payload: &[] };

        process_gps_event(&mut gps, &mut gps_status, GpsParserEvent::UbxMessage(message));

        assert_eq!(gps, GpsSolution::default());
    }
}