gps.rs

use core::fmt::{Debug, Display};
use half::f16;

#[derive(PartialEq, Clone)]
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 = if heading >= 0.0 {
            heading % 360.0
        } else {
            // slightly hacky no-std floor
            let factor = (-heading / 360.0) as u32 as f64;
            360.0 * (1.0 + factor) + heading
        };
        let heading = round(heading * 128.0 / 180.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 * 180.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,
        })
    }
}

impl Debug for Gps {
    fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result {
        f.debug_struct("Gps")
            .field("latitude", &DebugUnits(self.latitude(), "°"))
            .field("longitude", &DebugUnits(self.longitude(), "°"))
            .field("altitude", &DebugUnits(self.altitude, " m"))
            .field("max_error", &DebugUnits(self.max_error, " m"))
            .field("heading", &DebugUnits(self.heading(), "°"))
            .field("speed", &DebugUnits(self.speed, " m/s"))
            .finish()
    }
}

struct DebugUnits<'a, T>(T, &'a str);

impl<T: Display> Debug for DebugUnits<'_, T> {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "{}{}", self.0, self.1)
    }
}

// no-std and it's not worth bringing in a library for this
fn round(v: f64) -> u32 {
    let floor = v as u32;
    let delta = v - floor as f64;
    if delta <= 0.5 {
        floor
    } else {
        floor + 1
    }
}

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

    #[test]
    fn heading_is_correct() {
        let gps = Gps::new(0.0, 0.0, f16::from_f32(0.0), 0, 359.0, f16::from_f32(0.0));
        assert_eq!(gps.heading, 255);

        let gps = Gps::new(0.0, 0.0, f16::from_f32(0.0), 0, 0.0, f16::from_f32(0.0));
        assert_eq!(gps.heading, 0);

        let gps = Gps::new(0.0, 0.0, f16::from_f32(0.0), 0, -20.0, f16::from_f32(0.0));
        assert_eq!(gps.heading, 242);

        let gps = Gps::new(0.0, 0.0, f16::from_f32(0.0), 0, 719.0, f16::from_f32(0.0));
        assert_eq!(gps.heading, 255);

        let gps = Gps::new(0.0, 0.0, f16::from_f32(0.0), 0, 180.0, f16::from_f32(0.0));
        assert_eq!(gps.heading, 128);

        let gps = Gps::new(0.0, 0.0, f16::from_f32(0.0), 0, 540.0, f16::from_f32(0.0));
        assert_eq!(gps.heading, 128);
    }

    #[test]
    fn debug_printing() {
        let gps = Gps::new(0.0, 0.0, f16::from_f32(0.0), 0, 359.0, f16::from_f32(0.0));
        let x = format!("{gps:?}");
        assert_eq!(x,"Gps { latitude: 0°, longitude: 0°, altitude: 0 m, max_error: 0 m, heading: 358.59375°, speed: 0 m/s }")
    }
}