flight-computer 0.1.0

//! Control the GNSS receiver
use bbqueue::{Consumer, GrantR};
use embedded_sdmmc::Timestamp;
use fugit::{ExtU64, MillisDuration};
use ublox::{
    AlignmentToReferenceTime, CfgMsgSinglePortBuilder, CfgNav5DynModel, CfgNav5FixMode,
    CfgNav5Params, CfgNav5UtcStandard, FixedLinearBuffer, NavPosVelTime, ParserError,
};

/// ublox parser static buffer length
pub const PARSER_BUFFER_LEN: usize = 1024;

/// Log messages at 17.86 Hz in release mode
#[cfg(not(debug_assertions))]
pub const ACTIVE_MESSAGE_RATE: u16 = 56;

/// Log messages at 10 Hz in debug mode
#[cfg(debug_assertions)]
pub const ACTIVE_MESSAGE_RATE: u16 = 100;

/// On startup, receive messages at 1 Hz
pub const STARTUP_MESSAGE_RATE: u16 = 1000;

#[derive(Debug)]
pub enum Error {
    InvalidFix,
    ParserError(ParserError),
}

impl From<ParserError> for Error {
    fn from(item: ParserError) -> Error {
        Error::ParserError(item)
    }
}

#[derive(Clone)]
pub struct HeaderData {
    /// The UTC timestamp
    pub utc: embedded_sdmmc::Timestamp,
    /// The millisecond timestamp since reset
    pub timestamp: MillisDuration<u64>,
}

impl core::default::Default for HeaderData {
    fn default() -> Self {
        HeaderData {
            utc: Timestamp {
                year_since_1970: 0,
                zero_indexed_month: 0,
                zero_indexed_day: 0,
                hours: 0,
                minutes: 0,
                seconds: 0,
            },
            timestamp: 0.millis(),
        }
    }
}

pub trait UbloxDriver {
    /// Enable GNSS circuitry
    fn enable_gnss(&mut self);

    /// Disable GNSS circuitry
    fn disable_gnss(&mut self);

    /// Send a byte array to the device
    fn send_blocking(&mut self, packet: &[u8]);
}

/// Driver for the u-blox SAM-M8Q module
pub struct Device<D: UbloxDriver, const LEN: usize> {
    /// Device driver
    driver: D,
    /// UBX message parser
    parser: ublox::Parser<FixedLinearBuffer<'static>>,
    /// Incoming RX messages consumer
    rx_cons: Consumer<'static, LEN>,
    /// Has the signal been acquired since the device is on?
    signal_acquired: bool,
    /// The latest file header data received
    header_data: Option<HeaderData>,
}

impl<D: UbloxDriver, const LEN: usize> Device<D, LEN> {
    /// Create new device
    #[inline]
    pub fn new(
        driver: D,
        rx_cons: Consumer<'static, LEN>,
        buffer: &'static mut [u8; PARSER_BUFFER_LEN],
    ) -> Self {
        let buffer = FixedLinearBuffer::new(buffer);

        let mut device = Device {
            driver,
            rx_cons,
            parser: ublox::Parser::new(buffer),
            signal_acquired: false,
            header_data: None,
        };

        device.gnss_power(true);

        // Configure dynamic model (UBX-CFG-NAV5)
        device.driver.send_blocking(
            &ublox::CfgNav5Builder {
                mask: CfgNav5Params::DYN | CfgNav5Params::POS_FIX_MODE,
                dyn_model: CfgNav5DynModel::AirborneWith4gAcceleration,
                fix_mode: CfgNav5FixMode::Auto2D3D,
                fixed_alt: 0.0,
                fixed_alt_var: 1.0,
                min_elev_degrees: 5,
                dr_limit: 0,
                pdop: 25.0,
                tdop: 25.0,
                pacc: 100,
                tacc: 100,
                static_hold_thresh: 0.0,
                dgps_time_out: 60,
                cno_thresh_num_svs: 0,
                cno_thresh: 0,
                reserved1: [0; 2],
                static_hold_max_dist: 0,
                utc_standard: CfgNav5UtcStandard::Automatic,
                reserved2: [0; 5],
            }
            .into_packet_bytes(),
        );

        // Configure Position/Velocity/Time message output (UBX-CFG-MSG)
        // Send a UBX-NAV-PVT message every solution
        device.driver.send_blocking(
            &CfgMsgSinglePortBuilder::set_rate_for::<NavPosVelTime>(1).into_packet_bytes(),
        );

        /*
        // Configure navigation status message output (UBX-CFG-MSG)
        self.driver.send_blocking(
            &CfgMsgSinglePortBuilder::set_rate_for::<NavStatus>(1).into_packet_bytes(),
        );
        */

        device
    }

    /// Set the navigation message rate in full power mode
    #[inline]
    pub fn full_power(&mut self) {
        let power_setup = cfg_pms::PowerSetupValue::FullPower;
        self.driver.send_blocking(
            &cfg_pms::CfgPms::new()
                .with_power_setup_value(power_setup)
                .into_bytes(),
        );

        // Configure message rate (UBX-CFG-RATE)
        // 56 ms (17.85 Hz) is the fastest we seem to be able to get out of the SAM-M8Q
        // receiver.
        self.driver.send_blocking(
            &ublox::CfgRateBuilder {
                measure_rate_ms: ACTIVE_MESSAGE_RATE,
                nav_rate: 1,
                time_ref: AlignmentToReferenceTime::Utc,
            }
            .into_packet_bytes(),
        );
    }

    /// Enter low-power mode. The message rate is set to 1 Hz in this mode.
    #[inline]
    pub fn low_power_1hz(&mut self) {
        let power_setup = cfg_pms::PowerSetupValue::Aggressive1Hz;
        self.driver.send_blocking(
            &cfg_pms::CfgPms::new()
                .with_power_setup_value(power_setup)
                .into_bytes(),
        );
    }

    /// Enable or disable the receiver
    #[inline]
    pub fn gnss_power(&mut self, enabled: bool) {
        self.signal_acquired = false;
        if enabled {
            self.driver.enable_gnss();
        } else {
            self.driver.disable_gnss();
        }
    }

    /// Parse received data. This is an unfortunate workaround because of how
    /// lifetimes work. How to use this function:
    ///
    /// ```
    /// use flight_computer::ublox_device::{UbloxDriver, Device};
    ///
    /// fn do_parse<D: UbloxDriver, const LEN: usize>(gps: &mut Device<D, LEN>) -> Result<(), bbqueue::Error>{
    ///     let (parser, grant) = gps.parse()?;
    ///     let mut iter = parser.consume(&grant);
    ///
    ///     while let Some(packet) = iter.next() {
    ///         // Do something with the packet here
    ///     }
    ///
    ///     Ok(())
    /// }
    /// ```
    ///
    /// # Errors
    ///
    /// May return `Err` if there is a problem while trying to acquire a
    /// `bbqueue` grant.
    #[inline]
    pub fn parse(
        &mut self,
    ) -> Result<
        (
            &mut ublox::Parser<FixedLinearBuffer<'static>>,
            GrantR<'static, LEN>,
        ),
        bbqueue::Error,
    > {
        let mut grant = self.rx_cons.read()?;
        grant.to_release(usize::MAX);
        Ok((&mut self.parser, grant))
    }

    /// Has the signal been acquired since the device is on?
    #[inline]
    pub fn signal_acquired(&self) -> bool {
        self.signal_acquired
    }

    /// Notify that a valid signal has been acquired
    #[inline]
    pub fn acquire_signal(&mut self) {
        self.signal_acquired = true;
    }

    /// Set header data
    #[inline]
    pub fn set_header_data(&mut self, data: &HeaderData) {
        self.header_data.replace(data.clone());
    }

    /// Return header data if it is valid.
    #[inline]
    pub fn header_data(&self) -> Option<HeaderData> {
        self.header_data.clone()
    }

    /// Invalidate header data
    #[inline]
    pub fn invalidate_header_data(&mut self) {
        self.header_data = None;
    }
}

#[derive(Debug)]
pub struct GnssData {
    pub lat: i32,
    pub lon: i32,
    pub ground_speed: u32,
    pub vel_down: i32,
    pub heading: Option<i32>,
}

pub mod cfg_pms {
    use modular_bitfield::prelude::*;

    #[bitfield]
    pub struct CfgPms {
        #[skip(setters)]
        pub version: u8,
        pub power_setup_value: PowerSetupValue,
        pub period: u16,
        pub on_time: u16,
        #[skip]
        _reserved: B16,
    }

    #[derive(BitfieldSpecifier)]
    #[bits = 8]
    pub enum PowerSetupValue {
        FullPower = 0x00,
        Balanced = 0x01,
        Interval = 0x02,
        Aggressive1Hz = 0x03,
        Aggressive2Hz = 0x04,
        Aggressive4Hz = 0x05,
        Invalid = 0xFF,
    }
}

pub mod cfg_prt_uart {
    #![allow(clippy::identity_op)]
    #![allow(unused_braces)]

    use modular_bitfield::prelude::*;
    #[bitfield]
    #[repr(u16)]
    pub struct TxReady {
        pub en: bool,
        pub pol: Polarity,
        pub pin: B5,
        pub thres: B9,
    }

    #[bitfield]
    #[repr(u32)]
    pub struct Mode {
        #[skip]
        _reserved: B6,
        pub char_len: CharLen,
        #[skip]
        _reserved: B1,
        pub parity: Parity,
        pub n_stop_bits: NumStopBits,
        #[skip]
        _reserved: B18,
    }

    #[bitfield]
    #[repr(u16)]
    pub struct ProtoMask {
        pub ubx: bool,
        pub nmea: bool,
        pub rtcm: bool,
        #[skip]
        _reserved: B2,
        pub rtcm3: bool,
        #[skip]
        _reserved: B10,
    }

    #[bitfield]
    #[repr(u16)]
    pub struct Flags {
        #[skip]
        _reserved: B1,
        pub extended_tx_timeout: bool,
        #[skip]
        _reserved: B14,
    }

    #[derive(BitfieldSpecifier)]
    #[bits = 2]
    pub enum NumStopBits {
        Bits1 = 0b00,
        Bits1p5 = 0b01,
        Bits2 = 0b10,
        Bits0p5 = 0b11,
    }

    #[derive(BitfieldSpecifier)]
    #[bits = 2]
    pub enum CharLen {
        Bits7 = 0b10,
        Bits8 = 0b11,
    }

    #[derive(BitfieldSpecifier)]
    #[bits = 3]
    pub enum Parity {
        Even = 0b000,
        Odd = 0b001,
        None = 0b100,
    }

    #[derive(BitfieldSpecifier)]
    #[bits = 1]
    pub enum Polarity {
        HighActive = 0,
        LowActive = 1,
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use cfg_prt_uart::*;

    use ublox::{
        AlignmentToReferenceTime, CfgNav5DynModel, CfgNav5FixMode, CfgNav5Params,
        CfgNav5UtcStandard,
    };

    #[test]
    fn build_packet_prt() {
        const BUS_SPEED: u32 = 9600_u32;

        let packet = ublox::CfgPrtUartBuilder {
            portid: ublox::UartPortId::Uart1,
            reserved0: 0,
            // TODO Configuration of the TX Ready
            tx_ready: TxReady::new().with_en(false).into(),
            mode: Mode::new()
                .with_char_len(CharLen::Bits8)
                .with_parity(Parity::None)
                .with_n_stop_bits(NumStopBits::Bits1)
                .into(),
            baud_rate: BUS_SPEED,
            in_proto_mask: ProtoMask::new().with_ubx(true).into(),
            out_proto_mask: ProtoMask::new().with_ubx(true).with_nmea(true).into(),
            flags: Flags::new().into(),
            reserved5: 0,
        }
        .into_packet_bytes();

        let frame = [
            0xb5, 0x62, 0x6, 0x0, 0x14, 0x0, 0x1, 0x0, 0x0, 0x0, 0xc0, 0x8, 0x0, 0x0, 0x80, 0x25,
            0x0, 0x0, 0x1, 0x0, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8c, 0x85,
        ];

        assert_eq!(packet, frame);
    }

    #[test]
    fn build_packet_nav5() {
        let packet = ublox::CfgNav5Builder {
            mask: CfgNav5Params::DYN | CfgNav5Params::POS_FIX_MODE,
            dyn_model: CfgNav5DynModel::AirborneWith4gAcceleration,
            fix_mode: CfgNav5FixMode::Auto2D3D,
            fixed_alt: 0.0,
            fixed_alt_var: 0.0,
            min_elev_degrees: 0,
            dr_limit: 0,
            pdop: 0.0,
            tdop: 0.0,
            pacc: 0,
            tacc: 0,
            static_hold_thresh: 0.0,
            dgps_time_out: 0,
            cno_thresh_num_svs: 0,
            cno_thresh: 0,
            reserved1: [0; 2],
            static_hold_max_dist: 0,
            utc_standard: CfgNav5UtcStandard::Automatic,
            reserved2: [0; 5],
        }
        .into_packet_bytes();

        let frame = [
            0xb5, 0x62, 0x6, 0x24, 0x24, 0x0, 0x5, 0x0, 0x8, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
            0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
            0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x5e, 0xeb,
        ];

        assert_eq!(packet, frame);
    }

    #[test]
    fn build_packet_rate() {
        let packet = ublox::CfgRateBuilder {
            measure_rate_ms: 50,
            nav_rate: 1,
            time_ref: AlignmentToReferenceTime::Utc,
        }
        .into_packet_bytes();

        let frame = [
            0xb5, 0x62, 0x6, 0x8, 0x6, 0x0, 0x32, 0x0, 0x1, 0x0, 0x0, 0x0, 0x47, 0xe4,
        ];

        assert_eq!(packet, frame);
    }

    /// Test that `ublox::Parser` can receive partially formed packets and
    /// release them when the rest is sent
    #[test]
    fn send_partial_packets() {
        let mut buffer = [0u8; 512];
        let buffer = ublox::FixedLinearBuffer::new(&mut buffer);
        let mut parser = ublox::Parser::new(buffer);

        {
            // Partial AckAck packet
            let packet_1 = [0xb5, 0x62, 0x05, 0x01, 0x02];
            let mut iter = parser.consume(&packet_1);
            println!("Print packet 1 (partial AckAck): ");

            if iter.next().is_some() {
                panic!("Returned some packet when only a partial packet was supplied");
            }
        }

        {
            // Finish sending AckAck packet, send partial AckNak packet
            let packet_2 = [0x00, 0x06, 0x00, 0x0e, 0x37, 0xb5, 0x62, 0x05, 0x00, 0x02];
            // Finish AckAck here --------------------^    ^
            // Start new packet here ----------------------|
            let mut iter = parser.consume(&packet_2);
            println!("Print packet 2 (finish AckAck, partial AckNak): ");
            match iter.next() {
                Some(Ok(ublox::PacketRef::AckAck(_))) => (),
                _ => panic!("Did not return expected packet"),
            }
        }

        {
            // Finish sending AckNak packet
            let packet_3 = [0x00, 0x06, 0x00, 0x0d, 0x32];
            let mut iter = parser.consume(&packet_3);
            println!("Print packet 3 (finish AckNak): ");
            match iter.next() {
                Some(Ok(ublox::PacketRef::AckNak(_))) => (),
                _ => panic!("Did not return expected packet"),
            }
        }
    }
}