1
2
3
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
96
97
98
99
100
101
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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
use units::Radians;
macro_rules! interpolate {
($lhs:ident, $rhs:ident, $factor:ident, $var:ident) => {{
$lhs.$var + $factor * ($rhs.$var - $lhs.$var)
}}
}
macro_rules! interpolate_optional {
($lhs:ident, $rhs:ident, $factor:ident, $var:ident) => {{
if let Some(l) = $lhs.$var {
if let Some(r) = $rhs.$var {
Some(l + $factor * (r - l))
} else {
None
}
} else {
None
}
}}
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
#[allow(missing_docs)]
pub struct Point {
pub time: f64,
pub longitude: Radians<f64>,
pub latitude: Radians<f64>,
pub altitude: f64,
pub roll: Radians<f64>,
pub pitch: Radians<f64>,
pub yaw: Radians<f64>,
pub distance: Option<f64>,
pub x_velocity: Option<f64>,
pub y_velocity: Option<f64>,
pub z_velocity: Option<f64>,
pub wander_angle: Option<Radians<f64>>,
pub x_acceleration: Option<f64>,
pub y_acceleration: Option<f64>,
pub z_acceleration: Option<f64>,
pub x_angular_rate: Option<Radians<f64>>,
pub y_angular_rate: Option<Radians<f64>>,
pub z_angular_rate: Option<Radians<f64>>,
pub accuracy: Option<Accuracy>,
}
impl Point {
pub fn interpolate(&self, other: &Point, time: f64) -> Point {
let factor = (time - self.time) / (other.time - self.time);
Point {
time: interpolate!(self, other, factor, time),
longitude: interpolate!(self, other, factor, longitude),
latitude: interpolate!(self, other, factor, latitude),
altitude: interpolate!(self, other, factor, altitude),
roll: interpolate!(self, other, factor, roll),
pitch: interpolate!(self, other, factor, pitch),
yaw: interpolate!(self, other, factor, yaw),
distance: interpolate_optional!(self, other, factor, distance),
x_velocity: interpolate_optional!(self, other, factor, x_velocity),
y_velocity: interpolate_optional!(self, other, factor, y_velocity),
z_velocity: interpolate_optional!(self, other, factor, z_velocity),
wander_angle: interpolate_optional!(self, other, factor, wander_angle),
x_acceleration: interpolate_optional!(self, other, factor, x_acceleration),
y_acceleration: interpolate_optional!(self, other, factor, y_acceleration),
z_acceleration: interpolate_optional!(self, other, factor, z_acceleration),
x_angular_rate: interpolate_optional!(self, other, factor, x_angular_rate),
y_angular_rate: interpolate_optional!(self, other, factor, y_angular_rate),
z_angular_rate: interpolate_optional!(self, other, factor, z_angular_rate),
accuracy: if let Some(a1) = self.accuracy {
if let Some(a2) = other.accuracy {
Some(a1.interpolate(&a2, time))
} else {
None
}
} else {
None
},
}
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
#[allow(missing_docs)]
pub struct Accuracy {
pub time: f64,
pub x: f64,
pub y: f64,
pub z: f64,
pub roll: Radians<f64>,
pub pitch: Radians<f64>,
pub yaw: Radians<f64>,
pub pdop: f64,
pub satellite_count: Option<SatelliteCount>,
}
impl Accuracy {
pub fn interpolate(&self, other: &Accuracy, time: f64) -> Accuracy {
let factor = (time - self.time) / (other.time - self.time);
Accuracy {
time: interpolate!(self, other, factor, time),
x: interpolate!(self, other, factor, x),
y: interpolate!(self, other, factor, y),
z: interpolate!(self, other, factor, z),
roll: interpolate!(self, other, factor, roll),
pitch: interpolate!(self, other, factor, pitch),
yaw: interpolate!(self, other, factor, yaw),
pdop: interpolate!(self, other, factor, pdop),
satellite_count: None,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum SatelliteCount {
Unspecified(u16),
Specified {
gps: u16,
glonass: u16,
},
}
impl Default for SatelliteCount {
fn default() -> SatelliteCount {
SatelliteCount::Unspecified(0)
}
}