gps.rs

use half::f16;

#[derive(Debug, PartialEq)]
pub struct Gps {
    latitude: i32,
    longitude: i32,
    pub altitude: f16,
    pub max_error: u8,
    heading: u8,
    pub speed: f16,
}

impl Gps {
    /// Constructs a new GPS whisker.
    ///
    /// latitude: degrees
    /// longitude: degrees
    /// altitude: meters
    /// max_error: maximum error distance from specified lat/lon/alt, meters
    /// heading: direction relative to north, degrees
    /// speed: meters per second
    pub fn new(
        latitude: f64,
        longitude: f64,
        altitude: f16,
        max_error: u8,
        heading: f64,
        speed: f16,
    ) -> Self {
        let latitude = latitude.clamp(-89.999, 89.999);
        let latitude = (latitude * ((1u32 << 31) as f64) / 90.0) as i32;

        let longitude = longitude.clamp(-179.999, 179.999);
        let longitude = (longitude * ((1u32 << 31) as f64) / 180.0) as i32;

        let heading = heading.clamp(0.0, 359.999);
        let heading = (heading * 128.0 / 360.0) as u8;

        Self {
            latitude,
            longitude,
            altitude,
            max_error,
            heading,
            speed,
        }
    }

    pub fn latitude(&self) -> f64 {
        (self.latitude as f64) / (1u32 << 31) as f64 * 90.0
    }

    pub fn longitude(&self) -> f64 {
        (self.longitude as f64) / (1u32 << 31) as f64 * 180.0
    }

    pub fn heading(&self) -> f64 {
        self.heading as f64 / 128.0 * 360.0
    }

    pub fn encode<'a>(&self, buf: &'a mut [u8]) -> Option<&'a [u8]> {
        let buf = buf.get_mut(0..15)?;
        buf[0] = 14;
        buf[1..5].copy_from_slice(&self.latitude.to_le_bytes());
        buf[5..9].copy_from_slice(&self.longitude.to_le_bytes());
        buf[9..11].copy_from_slice(&self.altitude.to_le_bytes());
        buf[11] = self.max_error;
        buf[12] = self.heading;
        buf[13..15].copy_from_slice(&self.speed.to_le_bytes());

        Some(buf)
    }

    pub fn decode(data: &[u8]) -> Option<Self> {
        let data = data.get(0..15)?;
        if data[0] != 14 {
            return None;
        }
        let latitude = i32::from_le_bytes(data[1..5].try_into().unwrap());
        let longitude = i32::from_le_bytes(data[5..9].try_into().unwrap());
        let altitude = f16::from_le_bytes(data[9..11].try_into().unwrap());
        let max_error = data[11];
        let heading = data[12];
        let speed = f16::from_le_bytes(data[13..15].try_into().unwrap());

        Some(Self {
            latitude,
            longitude,
            altitude,
            max_error,
            heading,
            speed,
        })
    }
}