protoflight 0.1.2

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
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

use vqm::Vector3f32;

use crate::gps::{GeographicCoordinate, GpsSolutionData};

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct GpsPositionMeters {
    pub position: Vector3f32,
}

impl GpsPositionMeters {
    pub const fn new() -> Self {
        Self { position: Vector3f32 { x: 0.0, y: 0.0, z: 0.0 } }
    }
}

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

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct GpsPositionLongLatAlt {
    pub longitude_degrees_x1e7: i32,
    pub latitude_degrees_x1e7: i32,
    pub altitude_cm: i32,
}

impl GpsPositionLongLatAlt {
    pub const fn new() -> Self {
        Self { longitude_degrees_x1e7: 0, latitude_degrees_x1e7: 0, altitude_cm: 0 }
    }
}

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

impl From<GpsPositionLongLatAlt> for GeographicCoordinate {
    #[inline]
    #[allow(clippy::cast_precision_loss)]
    fn from(position: GpsPositionLongLatAlt) -> Self {
        Self {
            longitude_degrees: (position.longitude_degrees_x1e7 as f32) * 1e-7,
            latitude_degrees: (position.latitude_degrees_x1e7 as f32) * 1e-7,
            altitude_meters: (position.altitude_cm as f32) * 0.1,
        }
    }
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct GpsData {
    pub position: GpsPositionLongLatAlt,
    pub distance_to_home_meters: f32,
    pub bearing_to_home_degrees: f32,
    pub distance_flown_meters: f32,
    pub time_of_week_ms: u32,
    pub velocity_north_cmps: i16,
    pub velocity_east_cmps: i16,
    pub velocity_down_cmps: i16,
    pub speed3d_cmps: i16,
    pub ground_speed_cmps: i16,
    pub heading_deci_degrees: i16,
    pub dilution_of_precision_positional: i16,
    pub satellite_count: u8,
    pub fix: u8,
    pub is_healthy: u8,
    pub update: u8,
}

#[allow(unused)]
impl GpsData {
    const FIX_HOME: u8 = 0x01;
    const FIX: u8 = 0x02;
    const FIX_EVER: u8 = 0x04;

    pub const fn new() -> Self {
        Self {
            position: GpsPositionLongLatAlt::new(),
            distance_to_home_meters: 0.0,
            bearing_to_home_degrees: 0.0,
            distance_flown_meters: 0.0,
            time_of_week_ms: 0,
            velocity_north_cmps: 0,
            velocity_east_cmps: 0,
            velocity_down_cmps: 0,
            speed3d_cmps: 0,
            ground_speed_cmps: 0,
            heading_deci_degrees: 0,
            dilution_of_precision_positional: 0,
            satellite_count: 0,
            fix: 0,
            is_healthy: 0,
            update: 0,
        }
    }
}

impl Default for GpsData {
    fn default() -> Self {
        Self::new()
    }
}
#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "std", derive(derive_more::Display))]
#[cfg_attr(feature = "std", display("Gps{{yaw_rate: {yaw_heading_radians}}}"))]
pub struct GpsYawHeadingMessage {
    pub yaw_heading_radians: f32,
    pub delta_t: f32,
}

impl GpsYawHeadingMessage {
    pub const fn new() -> Self {
        Self { yaw_heading_radians: 0.0, delta_t: 0.1 }
    }
}

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

#[derive(Clone, Copy, Debug, PartialEq)]
#[non_exhaustive]
pub enum GpsMessage {
    Gps(GpsData),
    GpsPositionMeters(GpsPositionMeters),
    GpsSolution(GpsSolutionData),
}

#[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>() {}
    fn is_full_no_default<T: Sized + Send + Sync + Unpin + Copy + Clone + PartialEq>() {}

    #[test]
    fn normal_types() {
        is_full::<GpsPositionMeters>();
        is_full::<GpsPositionLongLatAlt>();
        is_full::<GpsData>();
        is_full::<GpsYawHeadingMessage>();
        is_full_no_default::<GpsMessage>();
    }
}