protoflight 0.1.3

Protoflight flight controller.
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

use vqm::Vector3f32;

#[allow(unused)]
use vqm::MathMethods;

#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "std", derive(derive_more::Display))]
#[cfg_attr(feature = "std", display("Gps{{long:{longitude_degrees}, lat:{latitude_degrees}, alt:{altitude_meters}}}"))]
pub struct GeographicCoordinate {
    pub longitude_degrees: f32,
    pub latitude_degrees: f32,
    pub altitude_meters: f32,
}

impl Default for GeographicCoordinate {
    fn default() -> Self {
        Self::new(0.0, 0.0, 0.0)
    }
}

impl GeographicCoordinate {
    pub const fn new(longitude_degrees: f32, latitude_degrees: f32, altitude_meters: f32) -> Self {
        Self { longitude_degrees, latitude_degrees, altitude_meters }
    }
    pub const fn from_long_lat_alt(longitude_degrees_x1e7: i32, latitude_degrees_x1e7: i32, altitude_cm: i32) -> Self {
        #[allow(clippy::cast_precision_loss)]
        Self {
            longitude_degrees: (longitude_degrees_x1e7 as f32) * 1e-7,
            latitude_degrees: (latitude_degrees_x1e7 as f32) * 1e-7,
            altitude_meters: (altitude_cm as f32) * 0.1,
        }
    }
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Geodetic {
    home: GeographicCoordinate,
    k_latitude: f32,
    k_longitude: f32,
}

impl Geodetic {
    pub const fn new() -> Self {
        Self { home: GeographicCoordinate::new(0.0, 0.0, 0.0), k_latitude: 0.0, k_longitude: 0.0 }
    }
}

impl Default for Geodetic {
    fn default() -> Self {
        Self::new()
    }
}

#[allow(unused)]
impl Geodetic {
    pub const WGS84_EQUATORIAL_RADIUS_METERS: f32 = 6_378_137.0;

    pub fn set_home(&mut self, median: GeographicCoordinate) {
        self.home = median;

        let home_lat = self.home.latitude_degrees.to_radians();
        // https://en.wikipedia.org/wiki/Geographical_distance
        self.k_latitude = 111_132.09 - 566.05 * (2.0 * home_lat).cos() + 1.20 * (4.0 * home_lat).cos();
        self.k_longitude =
            111_415.13 * (home_lat).cos() - 94.55 * (3.0 * home_lat).cos() + 0.12 * (5.0 * home_lat).cos();
    }

    pub fn latitude_distance_meters(self, delta_latitude: f32) -> f32 {
        self.k_latitude * delta_latitude
    }

    pub fn longitude_distance_meters(self, delta_longitude: f32) -> f32 {
        self.k_longitude * delta_longitude
    }

    pub fn distance_meters(self, from: GeographicCoordinate, to: GeographicCoordinate) -> Vector3f32 {
        Vector3f32 {
            x: self.k_latitude * (to.latitude_degrees - from.latitude_degrees),
            y: self.k_longitude * (to.longitude_degrees - from.longitude_degrees),
            z: to.altitude_meters - from.altitude_meters,
        }
    }

    pub fn position_relative_to_home_meters(self, geographic_coordinate: GeographicCoordinate) -> Vector3f32 {
        self.distance_meters(self.home, geographic_coordinate)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    fn _is_normal<T: Sized + Send + Sync + Unpin>() {}
    fn is_full<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + PartialEq>() {}

    #[test]
    fn normal_types() {
        is_full::<GeographicCoordinate>();
        is_full::<Geodetic>();
    }
}