Newer
Older
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
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
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,
})
}
}