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
use sguaba::{
Bearing, Coordinate, engineering::Orientation, math::RigidBodyTransform, system, systems::Wgs84,
};
use uom::si::f64::{Angle, Length};
use uom::si::{angle::degree, length::meter};
fn main() {
// FRD and NED systems are "local" coordinate systems, meaning a given
// coordinate in the FRD of one plane will have a completely different
// coordinate if it were to be expressed in the FRD of another. so, to
// guard against accidentally getting them mixed up, we construct a new
// type for this plane's FRD and NED:
// the pilot observes things in FRD of the plane
system!(struct PlaneFrd using FRD);
// the pilot's instruments indicate the plane's orientation in NED
system!(struct PlaneNed using NED);
// what the pilot saw:
let observation = Coordinate::<PlaneFrd>::from_bearing(
Bearing::builder()
// clockwise from forward
.azimuth(Angle::new::<degree>(20.))
// upwards from straight-ahead
.elevation(Angle::new::<degree>(10.))
.expect("elevation is in [-90°, 90°]")
.build(),
Length::new::<meter>(400.), // at this range
);
// where the plane was at the time (eg, from GPS):
let wgs84 = Wgs84::builder()
.latitude(Angle::new::<degree>(12.))
.expect("latitude is in [-90°, 90°]")
.longitude(Angle::new::<degree>(30.))
.altitude(Length::new::<meter>(1000.))
.build();
// where the plane was facing at the time (eg, from instrument panel);
// expressed in yaw, pitch, roll relative to North-East-Down:
let orientation_in_ned = Orientation::<PlaneNed>::tait_bryan_builder()
.yaw(Angle::new::<degree>(8.))
.pitch(Angle::new::<degree>(45.))
.roll(Angle::new::<degree>(0.))
.build();
// to convert between NED and ECEF, we need a transform between the two.
// this transform depends on where on the globe you are, so it takes the WGS84 position:
// SAFETY: we're claiming that `wgs84` is the location of `PlaneNed`'s origin.
let ecef_to_plane_ned = unsafe { RigidBodyTransform::ecef_to_ned_at(&wgs84) };
// to convert between FRD (which the observation was made in) and NED,
// we just need the plane's orientation, which we have from the instruments!
// SAFETY: we're claiming that the given NED orientation makes up the axes of `PlaneFrd`.
let plane_ned_to_plane_frd = unsafe { orientation_in_ned.map_as_zero_in::<PlaneFrd>() };
// these transformations can be chained to go from ECEF to NED.
// this chaining would fail to compile if you got the arguments wrong!
let ecef_to_plane_frd = ecef_to_plane_ned.and_then(plane_ned_to_plane_frd);
// this transform lets you go from ECEF to FRD, but transforms work both ways,
// so we can apply it in inverse to take our `Coordinate<PlaneFrd>` and produce
// a `Coordinate<Ecef>`:
let observation_in_ecef = ecef_to_plane_frd.inverse_transform(observation);
// we can then turn that into WGS84 lat/lon/altitude!
println!("{:?}", observation_in_ecef.to_wgs84());
}