protoflight 0.1.4

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

use super::{GpsProvider, UbxMessage, UbxParser, nmea::NmeaParser};

//
#[allow(unused)]
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum GpsParserEvent<'a> {
    NmeaComplete(&'a [u8]),
    UbxMessage(UbxMessage<'a>),
}

#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub enum GpsParser {
    Nmea(NmeaParser),
    Ubx(UbxParser),
    #[default]
    NoParser,
}

impl GpsParser {
    #[allow(unused)]
    pub const fn new(gps_provider: GpsProvider) -> Option<Self> {
        match gps_provider {
            GpsProvider::Nmea => Some(GpsParser::Nmea(NmeaParser::new())),
            GpsProvider::Ubx => Some(GpsParser::Ubx(UbxParser::new())),
            _ => None,
        }
    }

    pub const fn new_unwrapped(gps_provider: GpsProvider) -> Self {
        match gps_provider {
            GpsProvider::Ubx => GpsParser::Ubx(UbxParser::new()),
            _ => GpsParser::Nmea(NmeaParser::new()),
        }
    }
}

impl GpsParser {
    pub fn on_data_received(&mut self, data: u8) -> Option<GpsParserEvent<'_>> {
        match self {
            Self::Nmea(parser) => {
                if parser.on_data_received(data) {
                    parser.payload().map(GpsParserEvent::NmeaComplete)
                } else {
                    None
                }
            }
            Self::Ubx(parser) => parser.on_data_received(data).map(GpsParserEvent::UbxMessage),
            Self::NoParser => None,
        }
    }
}

#[cfg(test)]
mod tests {
    #![allow(clippy::panic)]

    use super::*;
    use crate::gps::ubx::UbxClassId;

    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::<GpsParser>();
    }
    fn make_ubx_frame(class: u8, id: u8, payload: &[u8]) -> Vec<u8> {
        #[allow(clippy::cast_possible_truncation)]
        let payload_len = (payload.len() as u16).to_le_bytes();
        let mut frame = Vec::with_capacity(8 + payload.len());
        frame.extend_from_slice(&[
            0xB5, 0x62, // UBX sync characters
            class, id,
        ]);
        frame.extend_from_slice(&payload_len);
        frame.extend_from_slice(payload);
        let mut ck_a = 0u8;
        let mut ck_b = 0u8;
        // Checksum covers class, ID, length and payload.
        for &byte in &frame[2..] {
            ck_a = ck_a.wrapping_add(byte);
            ck_b = ck_b.wrapping_add(ck_a);
        }
        frame.extend_from_slice(&[ck_a, ck_b]);
        frame
    }
    #[test]
    fn ubx_parser_produces_ubx_message_event() {
        let payload = [0x11, 0x22, 0x33];
        let frame = make_ubx_frame(0x01, 0x02, &payload);
        let mut parser = GpsParser::Ubx(UbxParser::new());
        for byte in frame {
            if let Some(event) = parser.on_data_received(byte) {
                match event {
                    GpsParserEvent::UbxMessage(message) => {
                        assert_eq!(message.class, UbxClassId::Nav);
                        assert_eq!(message.id, 0x02);
                        assert_eq!(message.payload, &payload);
                    }
                    GpsParserEvent::NmeaComplete(_) => {
                        panic!("expected UBX message event");
                    }
                }
                return;
            }
        }
        panic!("expected UBX message event");
    }
    #[test]
    fn nmea_parser_produces_nmea_complete_event() {
        let record = b"$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47\r\n";
        let mut parser = GpsParser::Nmea(NmeaParser::new());
        for &byte in record {
            if let Some(event) = parser.on_data_received(byte) {
                match event {
                    GpsParserEvent::NmeaComplete(payload) => {
                        assert_eq!(payload, b"GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,");
                    }
                    GpsParserEvent::UbxMessage(_) => {
                        panic!("expected NMEA complete event");
                    }
                }
                return;
            }
        }
        panic!("expected NMEA complete event");
    }
}