Newer
Older
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 {
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
}
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,
})
}
}
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
// 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);
}
}