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 }") } }