gnss_protos/gps/
decoder.rs

1use crate::gps::{
2    frame1::{
3        Word10 as Ephemeris1Word10, Word3 as Ephemeris1Word3, Word4 as Ephemeris1Word4,
4        Word5 as Ephemeris1Word5, Word6 as Ephemeris1Word6, Word7 as Ephemeris1Word7,
5        Word8 as Ephemeris1Word8, Word9 as Ephemeris1Word9,
6    },
7    frame2::{
8        Word10 as Ephemeris2Word10, Word3 as Ephemeris2Word3, Word4 as Ephemeris2Word4,
9        Word5 as Ephemeris2Word5, Word6 as Ephemeris2Word6, Word7 as Ephemeris2Word7,
10        Word8 as Ephemeris2Word8, Word9 as Ephemeris2Word9,
11    },
12    frame3::{
13        Word10 as Ephemeris3Word10, Word3 as Ephemeris3Word3, Word4 as Ephemeris3Word4,
14        Word5 as Ephemeris3Word5, Word6 as Ephemeris3Word6, Word7 as Ephemeris3Word7,
15        Word8 as Ephemeris3Word8, Word9 as Ephemeris3Word9,
16    },
17    GpsDataByte, GpsQzssFrame, GpsQzssFrameId, GpsQzssHow, GpsQzssSubframe, GpsQzssTelemetry,
18};
19
20#[cfg(feature = "log")]
21use log::{error, trace};
22
23#[derive(Default, Debug, Copy, Clone, PartialEq)]
24enum State {
25    /// Telemetry dword parsing
26    #[default]
27    Telemetry,
28
29    /// How dword parsing
30    How,
31
32    /// Data word parsing
33    DataWord,
34}
35
36pub struct GpsQzssDecoder {
37    /// Frame counter
38    ptr: usize,
39
40    /// data word storage
41    pub(crate) dword: u32,
42
43    /// total number of bits collected
44    pub(crate) collected: usize,
45
46    /// Current [State]
47    state: State,
48
49    /// Latest [GpsQzssTelemetry]
50    tlm: GpsQzssTelemetry,
51
52    /// Latest [GpsQzssHow]
53    how: GpsQzssHow,
54
55    /// Pending [GpsQzssSubframe]
56    subframe: GpsQzssSubframe,
57
58    /// u32 storage for words that overlap
59    storage: u32,
60
61    /// True if parity verification is requested
62    parity_check: bool,
63}
64
65impl Default for GpsQzssDecoder {
66    /// Creates a default [GpsQzssDecoder] with parity bit verification
67    fn default() -> Self {
68        let state = State::default();
69
70        Self {
71            state,
72            ptr: 0,
73            dword: 0,
74            collected: 0,
75            storage: 0,
76            parity_check: true,
77            tlm: Default::default(),
78            how: Default::default(),
79            subframe: Default::default(),
80        }
81    }
82}
83
84impl GpsQzssDecoder {
85    /// Parses a GPS/QZSS stream of [GpsDataByte]s, returns a [GpsQzssFrame] when
86    /// one is identified and the last data word has been correctly processed.
87    pub fn parse(&mut self, byte: GpsDataByte) -> Option<GpsQzssFrame> {
88        // collect bytes
89        match byte {
90            GpsDataByte::LsbPadded(byte) => {
91                self.collected += 6;
92                self.dword <<= 6;
93                self.dword |= (byte as u32) >> 2;
94            },
95            GpsDataByte::MsbPadded(byte) => {
96                self.collected += 6;
97                self.dword <<= 6;
98                self.dword |= (byte & 0x3f) as u32;
99            },
100            GpsDataByte::Byte(byte) => {
101                self.collected += 8;
102                self.dword <<= 8;
103                self.dword |= byte as u32;
104            },
105        }
106
107        #[cfg(feature = "log")]
108        trace!(
109            "GPS - data collection - ptr={} dword=0x{:08x}",
110            self.ptr,
111            self.dword
112        );
113
114        if self.collected < 30 {
115            // Can't proceed
116            return None;
117        }
118
119        // data word processing
120        let dword = self.dword;
121
122        let mut ret = Option::<GpsQzssFrame>::None;
123
124        // buffer decoding attempt
125        #[cfg(feature = "log")]
126        trace!(
127            "GPS - state={:?} - decoding dword=0x{:08x}",
128            self.state,
129            dword
130        );
131
132        match self.state {
133            State::Telemetry => match GpsQzssTelemetry::decode(dword) {
134                Ok(tlm) => {
135                    self.tlm = tlm;
136                    self.state = State::How;
137                    #[cfg(feature = "log")]
138                    trace!("GPS - TLM {:?}", tlm);
139                },
140                #[cfg(feature = "log")]
141                Err(e) => {
142                    error!("GPS - TLM decoding error: {:?}", e);
143                },
144                #[cfg(not(feature = "log"))]
145                Err(_) => {},
146            },
147
148            State::How => match GpsQzssHow::decode(dword) {
149                Ok(how) => {
150                    self.how = how;
151                    self.state = State::DataWord;
152                    self.ptr = 3;
153
154                    match how.frame_id {
155                        GpsQzssFrameId::Ephemeris1 => {
156                            self.subframe = GpsQzssSubframe::Ephemeris1(Default::default());
157                        },
158                        GpsQzssFrameId::Ephemeris2 => {
159                            self.subframe = GpsQzssSubframe::Ephemeris2(Default::default());
160                        },
161                        GpsQzssFrameId::Ephemeris3 => {
162                            self.subframe = GpsQzssSubframe::Ephemeris3(Default::default());
163                        },
164                    }
165                    #[cfg(feature = "log")]
166                    trace!("GPS - HOW {:?}", how);
167                },
168                #[cfg(feature = "log")]
169                Err(e) => {
170                    error!("GPS - HOW decoding error: {:?}", e);
171                    self.ptr = 0;
172                    self.dword = 0;
173                    self.state = State::default();
174                },
175                #[cfg(not(feature = "log"))]
176                Err(_) => {},
177            },
178
179            State::DataWord => {
180                match self.how.frame_id {
181                    GpsQzssFrameId::Ephemeris1 => match self.ptr {
182                        3 => {
183                            let word = Ephemeris1Word3::decode(self.dword);
184
185                            let frame = self.subframe.as_mut_eph1().expect("internal error");
186
187                            frame.week = word.week;
188                            frame.ura = word.ura;
189                            frame.ca_or_p_l2 = word.ca_or_p_l2;
190                            frame.health = word.health;
191                            frame.iodc |= (word.iodc_msb as u16) << 8;
192                            #[cfg(feature = "log")]
193                            trace!("GPS - EPH #1 Word#3 {:?}", word);
194                        },
195                        4 => {
196                            let word = Ephemeris1Word4::decode(self.dword);
197                            let frame = self.subframe.as_mut_eph1().expect("internal error");
198                            frame.l2_p_data_flag = word.l2_p_data_flag;
199                            frame.reserved_word4 = word.reserved;
200                            #[cfg(feature = "log")]
201                            trace!("GPS - EPH #1 Word#4 {:?}", word);
202                        },
203                        5 => {
204                            let word = Ephemeris1Word5::decode(self.dword);
205                            let frame = self.subframe.as_mut_eph1().expect("internal error");
206                            frame.reserved_word5 = word.reserved;
207                            #[cfg(feature = "log")]
208                            trace!("GPS - EPH #1 Word#5 {:?}", word);
209                        },
210                        6 => {
211                            let word = Ephemeris1Word6::decode(self.dword);
212                            let frame = self.subframe.as_mut_eph1().expect("internal error");
213                            frame.reserved_word6 = word.reserved;
214                            #[cfg(feature = "log")]
215                            trace!("GPS - EPH #1 Word#6 {:?}", word);
216                        },
217                        7 => {
218                            let word = Ephemeris1Word7::decode(self.dword);
219                            let frame = self.subframe.as_mut_eph1().expect("internal error");
220                            frame.tgd = (word.tgd as f64) / 2_f64.powi(31);
221                            frame.reserved_word7 = word.reserved;
222                            #[cfg(feature = "log")]
223                            trace!("GPS - EPH #1 Word#7 {:?}", word);
224                        },
225                        8 => {
226                            let word = Ephemeris1Word8::decode(self.dword);
227                            let frame = self.subframe.as_mut_eph1().expect("internal error");
228                            frame.toc = (word.toc as u32) * 16;
229                            frame.iodc |= word.iodc_lsb as u16;
230                            #[cfg(feature = "log")]
231                            trace!("GPS - EPH #1 Word#8 {:?}", word);
232                        },
233                        9 => {
234                            let word = Ephemeris1Word9::decode(self.dword);
235                            let frame = self.subframe.as_mut_eph1().expect("internal error");
236                            frame.af2 = (word.af2 as f64) / 2.0_f64.powi(55);
237                            frame.af1 = (word.af1 as f64) / 2.0_f64.powi(43);
238                            #[cfg(feature = "log")]
239                            trace!("GPS - EPH #1 Word#9 {:?}", word);
240                        },
241                        10 => {
242                            let word = Ephemeris1Word10::decode(self.dword);
243                            let frame = self.subframe.as_mut_eph1().expect("internal error");
244                            frame.af0 = (word.af0 as f64) / 2.0_f64.powi(31);
245                            #[cfg(feature = "log")]
246                            trace!("GPS - EPH #1 Word#10 {:?}", word);
247                        },
248                        _ => {
249                            unreachable!("invalid state");
250                        },
251                    },
252                    GpsQzssFrameId::Ephemeris2 => match self.ptr {
253                        3 => {
254                            let word = Ephemeris2Word3::decode(self.dword);
255                            let frame = self.subframe.as_mut_eph2().expect("internal error");
256                            frame.crs = (word.crs as f64) / 2.0_f64.powi(5);
257                            frame.iode = word.iode;
258                            #[cfg(feature = "log")]
259                            trace!("GPS - EPH #2 Word#3 {:?}", word);
260                        },
261                        4 => {
262                            let word = Ephemeris2Word4::decode(self.dword);
263                            let frame = self.subframe.as_mut_eph2().expect("internal error");
264                            frame.dn = (word.dn as f64) / 2.0_f64.powi(43);
265                            self.storage = word.m0_msb as u32;
266                            #[cfg(feature = "log")]
267                            trace!("GPS - EPH #2 Word#4 {:?}", word);
268                        },
269                        5 => {
270                            let word = Ephemeris2Word5::decode(self.dword);
271                            let frame = self.subframe.as_mut_eph2().expect("internal error");
272
273                            let mut m0 = self.storage;
274                            m0 <<= 24;
275                            m0 |= word.m0_lsb as u32;
276
277                            frame.m0 = ((m0 as i32) as f64) / 2.0_f64.powi(31);
278
279                            #[cfg(feature = "log")]
280                            trace!("GPS - EPH #2 Word#5 {:?}", word);
281                        },
282                        6 => {
283                            let word = Ephemeris2Word6::decode(self.dword);
284                            let frame = self.subframe.as_mut_eph2().expect("internal error");
285                            frame.cuc = (word.cuc as f64) / 2.0_f64.powi(29);
286                            self.storage = word.e_msb as u32;
287                            #[cfg(feature = "log")]
288                            trace!("GPS - EPH #2 Word#6 {:?}", word);
289                        },
290                        7 => {
291                            let word = Ephemeris2Word7::decode(self.dword);
292                            let frame = self.subframe.as_mut_eph2().expect("internal error");
293
294                            let mut e = self.storage;
295                            e <<= 24;
296                            e |= word.e_lsb;
297
298                            frame.e = (e as f64) / 2.0_f64.powi(33);
299                            #[cfg(feature = "log")]
300                            trace!("GPS - EPH #2 Word#7 {:?}", word);
301                        },
302                        8 => {
303                            let word = Ephemeris2Word8::decode(self.dword);
304                            let frame = self.subframe.as_mut_eph2().expect("internal error");
305
306                            frame.cus = (word.cus as f64) / 2.0_f64.powi(29);
307                            self.storage = word.sqrt_a_msb as u32;
308                            #[cfg(feature = "log")]
309                            trace!("GPS - EPH #2 Word#8 {:?}", word);
310                        },
311                        9 => {
312                            let word = Ephemeris2Word9::decode(self.dword);
313                            let frame = self.subframe.as_mut_eph2().expect("internal error");
314
315                            let mut sqrt_a = self.storage;
316                            sqrt_a <<= 24;
317                            sqrt_a |= word.sqrt_a_lsb;
318
319                            frame.sqrt_a = (sqrt_a as f64) / 2.0_f64.powi(19);
320
321                            #[cfg(feature = "log")]
322                            trace!("GPS - EPH #2 Word#9 {:?}", word);
323                        },
324                        10 => {
325                            let word = Ephemeris2Word10::decode(self.dword);
326                            let frame = self.subframe.as_mut_eph2().expect("internal error");
327
328                            frame.aodo = word.aodo;
329                            frame.fit_int_flag = word.fitint;
330                            frame.toe = (word.toe as u32) * 16;
331
332                            #[cfg(feature = "log")]
333                            trace!("GPS - EPH #2 Word#10 {:?}", word);
334                        },
335                        _ => {
336                            unreachable!("invalid state");
337                        },
338                    },
339                    GpsQzssFrameId::Ephemeris3 => match self.ptr {
340                        3 => {
341                            let word = Ephemeris3Word3::decode(self.dword);
342                            let frame = self.subframe.as_mut_eph3().expect("internal error");
343                            frame.cic = (word.cic as f64) / 2.0_f64.powi(29);
344                            self.storage = word.omega0_msb as u32;
345
346                            #[cfg(feature = "log")]
347                            trace!("GPS - EPH #3 Word#3 {:?}", word);
348                        },
349                        4 => {
350                            let word = Ephemeris3Word4::decode(self.dword);
351                            let frame = self.subframe.as_mut_eph3().expect("internal error");
352
353                            let mut omega0 = self.storage;
354                            omega0 <<= 24;
355                            omega0 |= word.omega0_lsb;
356
357                            frame.omega0 = ((omega0 as i32) as f64) / 2.0_f64.powi(31);
358
359                            #[cfg(feature = "log")]
360                            trace!("GPS - EPH #3 Word#4 {:?}", word);
361                        },
362                        5 => {
363                            let word = Ephemeris3Word5::decode(self.dword);
364                            let frame = self.subframe.as_mut_eph3().expect("internal error");
365
366                            frame.cis = (word.cis as f64) / 2.0_f64.powi(29);
367
368                            self.storage = word.i0_msb as u32;
369
370                            #[cfg(feature = "log")]
371                            trace!("GPS - EPH #2 Word#5 {:?}", word);
372                        },
373                        6 => {
374                            let word = Ephemeris3Word6::decode(self.dword);
375                            let frame = self.subframe.as_mut_eph3().expect("internal error");
376
377                            let mut i0 = self.storage;
378                            i0 <<= 24;
379                            i0 |= word.i0_lsb;
380
381                            frame.i0 = (i0 as f64) / 2.0_f64.powi(31);
382
383                            #[cfg(feature = "log")]
384                            trace!("GPS - EPH #3 Word#6 {:?}", word);
385                        },
386                        7 => {
387                            let word = Ephemeris3Word7::decode(self.dword);
388                            let frame = self.subframe.as_mut_eph3().expect("internal error");
389
390                            frame.crc = (word.crc as f64) / 2.0_f64.powi(5);
391                            self.storage = word.omega_msb as u32;
392
393                            #[cfg(feature = "log")]
394                            trace!("GPS - EPH #3 Word#7 {:?}", word);
395                        },
396                        8 => {
397                            let word = Ephemeris3Word8::decode(self.dword);
398                            let frame = self.subframe.as_mut_eph3().expect("internal error");
399
400                            let mut omega = self.storage;
401                            omega <<= 24;
402                            omega |= word.omega_lsb;
403
404                            frame.omega = ((omega as i32) as f64) / 2.0_f64.powi(31);
405
406                            #[cfg(feature = "log")]
407                            trace!("GPS - EPH #3 Word#8 {:?}", word);
408                        },
409                        9 => {
410                            let word = Ephemeris3Word9::decode(self.dword);
411                            let frame = self.subframe.as_mut_eph3().expect("internal error");
412
413                            frame.omega_dot = (word.omega_dot as f64) / 2.0_f64.powi(43);
414
415                            #[cfg(feature = "log")]
416                            trace!("GPS - EPH #3 Word#9 {:?}", word);
417                        },
418                        10 => {
419                            let word = Ephemeris3Word10::decode(self.dword);
420                            let frame = self.subframe.as_mut_eph3().expect("internal error");
421
422                            frame.idot = (word.idot as f64) / 2.0_f64.powi(43);
423                            frame.iode = word.iode;
424
425                            #[cfg(feature = "log")]
426                            trace!("GPS - EPH #3 Word#10 {:?}", word);
427                        },
428                        _ => {
429                            unreachable!("invalid state");
430                        },
431                    },
432                }
433
434                self.ptr += 1;
435            },
436        }
437
438        // reset
439        self.collected = 0;
440        self.dword = 0;
441
442        if self.ptr == 11 {
443            ret = Some(GpsQzssFrame {
444                how: self.how,
445                telemetry: self.tlm,
446                subframe: self.subframe,
447            });
448
449            self.ptr = 0;
450            self.state = State::Telemetry;
451        }
452
453        ret
454    }
455
456    /// Create a new [GpsQzssDecoder] that will not verify the parity bits
457    pub fn without_parity_verification(&self) -> Self {
458        Self {
459            ptr: self.ptr,
460            collected: 0,
461            dword: 0,
462            storage: 0,
463            state: self.state,
464            parity_check: false,
465            tlm: self.tlm.clone(),
466            how: self.how.clone(),
467            subframe: self.subframe.clone(),
468        }
469    }
470}
471
472#[cfg(test)]
473mod test {
474    use crate::{
475        gps::{GpsDataByte, GpsQzssDecoder, GpsQzssSubframe},
476        GpsQzssFrameId,
477    };
478
479    use crate::tests::from_ublox_be_bytes;
480
481    #[cfg(all(feature = "std", feature = "log"))]
482    use crate::tests::init_logger;
483
484    #[test]
485    fn test_tlm_decoding() {
486        #[cfg(all(feature = "std", feature = "log"))]
487        init_logger();
488
489        let bytes = [
490            GpsDataByte::Byte(0x8B),
491            GpsDataByte::Byte(0x00),
492            GpsDataByte::Byte(0x0D),
493            GpsDataByte::lsb_padded(0x00), // TLM
494        ];
495
496        let mut decoder = GpsQzssDecoder::default();
497
498        for byte in bytes {
499            let _ = decoder.parse(byte);
500        }
501
502        assert_eq!(decoder.tlm.message, 3);
503        assert_eq!(decoder.tlm.integrity, false);
504        assert_eq!(decoder.tlm.reserved_bits, true);
505    }
506
507    #[test]
508    fn test_ublox_tlm_decoding() {
509        #[cfg(all(feature = "std", feature = "log"))]
510        init_logger();
511
512        let bytes = [
513            GpsDataByte::msb_padded(0x22),
514            GpsDataByte::Byte(0xC1),
515            GpsDataByte::Byte(0x3E),
516            GpsDataByte::Byte(0x1B), // TLM
517        ];
518
519        let mut decoder = GpsQzssDecoder::default();
520
521        for byte in bytes {
522            let _ = decoder.parse(byte);
523        }
524
525        assert_eq!(decoder.tlm.message, 0x13E);
526        assert_eq!(decoder.tlm.integrity, false);
527        assert_eq!(decoder.tlm.reserved_bits, false);
528
529        let bytes = [0x22, 0xC1, 0x3E, 0x1B];
530        let bytes = from_ublox_be_bytes(&bytes);
531
532        let mut decoder = GpsQzssDecoder::default();
533
534        for byte in bytes {
535            let _ = decoder.parse(byte);
536        }
537
538        assert_eq!(decoder.tlm.message, 0x13E);
539        assert_eq!(decoder.tlm.integrity, false);
540        assert_eq!(decoder.tlm.reserved_bits, false);
541    }
542
543    #[test]
544    fn test_tlmhow_decoding() {
545        #[cfg(all(feature = "std", feature = "log"))]
546        init_logger();
547
548        let bytes = [
549            GpsDataByte::msb_padded(0x22),
550            GpsDataByte::Byte(0xC1),
551            GpsDataByte::Byte(0x3E),
552            GpsDataByte::Byte(0x1B), // TLM
553            GpsDataByte::MsbPadded(0x15),
554            GpsDataByte::Byte(0x27),
555            GpsDataByte::Byte(0xC9),
556            GpsDataByte::Byte(0x73), // HOW
557        ];
558
559        let mut decoder = GpsQzssDecoder::default();
560
561        for byte in bytes {
562            let _ = decoder.parse(byte);
563        }
564
565        assert_eq!(decoder.tlm.message, 0x13E);
566        assert_eq!(decoder.tlm.integrity, false);
567        assert_eq!(decoder.tlm.reserved_bits, false);
568
569        assert_eq!(decoder.how.alert, false);
570        assert_eq!(decoder.how.anti_spoofing, true);
571        assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1);
572    }
573
574    #[test]
575    fn test_ublox_tlmhow_decoding() {
576        #[cfg(all(feature = "std", feature = "log"))]
577        init_logger();
578
579        let bytes = [
580            GpsDataByte::MsbPadded(0x22),
581            GpsDataByte::Byte(0xC1),
582            GpsDataByte::Byte(0x3E),
583            GpsDataByte::Byte(0x1B), // TLM
584            GpsDataByte::MsbPadded(0x15),
585            GpsDataByte::Byte(0x27),
586            GpsDataByte::Byte(0xC9),
587            GpsDataByte::Byte(0x73), // HOW
588        ];
589
590        let mut decoder = GpsQzssDecoder::default();
591
592        for byte in bytes {
593            let _ = decoder.parse(byte);
594        }
595
596        assert_eq!(decoder.tlm.message, 0x13E);
597        assert_eq!(decoder.tlm.integrity, false);
598        assert_eq!(decoder.tlm.reserved_bits, false);
599
600        assert_eq!(decoder.how.alert, false);
601        assert_eq!(decoder.how.anti_spoofing, true);
602        assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1);
603
604        let bytes = from_ublox_be_bytes(&[0x22, 0xC1, 0x3E, 0x1B, 0x15, 0x27, 0xC9, 0x73]);
605
606        let mut decoder = GpsQzssDecoder::default();
607
608        for byte in bytes {
609            let _ = decoder.parse(byte);
610        }
611
612        assert_eq!(decoder.tlm.message, 0x13E);
613        assert_eq!(decoder.tlm.integrity, false);
614        assert_eq!(decoder.tlm.reserved_bits, false);
615
616        assert_eq!(decoder.how.alert, false);
617        assert_eq!(decoder.how.anti_spoofing, true);
618        assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1);
619    }
620
621    #[test]
622    fn test_incomplete_frame() {
623        #[cfg(all(feature = "std", feature = "log"))]
624        init_logger();
625
626        let mut found = false;
627
628        let bytes = [
629            GpsDataByte::Byte(0x8B),
630            GpsDataByte::Byte(0x00),
631            GpsDataByte::Byte(0x0D),
632            GpsDataByte::lsb_padded(0x00), // TLM
633        ];
634
635        let mut decoder = GpsQzssDecoder::default();
636
637        for byte in bytes {
638            if let Some(_) = decoder.parse(byte) {
639                found = true;
640            }
641        }
642
643        assert!(!found, "Invalid GPS frame decoded!");
644
645        let bytes = [
646            GpsDataByte::Byte(0x8B),
647            GpsDataByte::Byte(0x00),
648            GpsDataByte::Byte(0x0D),
649            GpsDataByte::lsb_padded(0x00), // TLM
650            GpsDataByte::Byte(0x8B),
651            GpsDataByte::Byte(0x00),
652            GpsDataByte::Byte(0x0D),
653            GpsDataByte::lsb_padded(0x00), // HOW
654            GpsDataByte::Byte(0x8B),
655            GpsDataByte::Byte(0x00),
656            GpsDataByte::Byte(0x0D),
657            GpsDataByte::lsb_padded(0x00), // WORD3
658            GpsDataByte::Byte(0x8B),
659            GpsDataByte::Byte(0x00),
660            GpsDataByte::Byte(0x0D),
661            GpsDataByte::lsb_padded(0x00), // WORD4
662            GpsDataByte::Byte(0x8B),
663            GpsDataByte::Byte(0x00),
664            GpsDataByte::Byte(0x0D),
665            GpsDataByte::lsb_padded(0x00), // WORD5
666            GpsDataByte::Byte(0x8B),
667            GpsDataByte::Byte(0x00),
668            GpsDataByte::Byte(0x0D),
669            GpsDataByte::lsb_padded(0x00), // WORD6
670            GpsDataByte::Byte(0x8B),
671            GpsDataByte::Byte(0x00),
672            GpsDataByte::Byte(0x0D),
673            GpsDataByte::lsb_padded(0x00), // WORD7
674            GpsDataByte::Byte(0x8B),
675            GpsDataByte::Byte(0x00),
676            GpsDataByte::Byte(0x0D),
677            GpsDataByte::lsb_padded(0x00), // WORD8
678            GpsDataByte::Byte(0x8B),
679            GpsDataByte::Byte(0x00),
680            GpsDataByte::Byte(0x0D),
681        ];
682
683        let mut decoder = GpsQzssDecoder::default();
684
685        for byte in bytes {
686            if let Some(_) = decoder.parse(byte) {
687                found = true;
688            }
689        }
690
691        assert!(!found, "Invalid GPS frame decoded");
692    }
693
694    #[test]
695    fn test_eph1_frame() {
696        #[cfg(all(feature = "std", feature = "log"))]
697        init_logger();
698
699        let mut found = false;
700
701        let bytes = from_ublox_be_bytes(&[
702            // TLM
703            0x22, 0xC1, 0x3E, 0x1B, // HOW
704            0x15, 0x27, 0xC9, 0x73, // WORD3
705            0x13, 0xE4, 0x00, 0x04, //WORD4
706            0x10, 0x4F, 0x5D, 0x31, //WORD5
707            0x97, 0x44, 0xE6, 0xD7, // WORD6
708            0x07, 0x75, 0x57, 0x83, //WORD7
709            0x33, 0x0C, 0x80, 0xB5, // WORD8
710            0x92, 0x50, 0x42, 0xA1, // WORD9
711            0x80, 0x00, 0x16, 0x84, //WORD10
712            0x31, 0x2C, 0x30, 0x33,
713        ]);
714
715        let mut decoder = GpsQzssDecoder::default().without_parity_verification();
716
717        for byte in bytes {
718            if let Some(frame) = decoder.parse(byte) {
719                assert_eq!(decoder.tlm.message, 0x13E);
720                assert_eq!(decoder.tlm.integrity, false);
721                assert_eq!(decoder.tlm.reserved_bits, false);
722
723                assert_eq!(decoder.how.alert, false);
724                assert_eq!(decoder.how.anti_spoofing, true);
725                assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1);
726
727                match frame.subframe {
728                    GpsQzssSubframe::Ephemeris1(frame1) => {
729                        assert_eq!(frame1.af2, 0.0);
730                        assert!((frame1.af1 - 1.023181539495e-011).abs() < 1e-14);
731                        assert!((frame1.af0 - -4.524961113930e-004).abs() < 1.0e-11);
732                        assert_eq!(frame1.week, 318);
733                        assert_eq!(frame1.toc, 266_400);
734                        assert_eq!(frame1.health, 0);
735                        found = true;
736                    },
737                    _ => panic!("incorrect subframe decoded!"),
738                }
739            }
740        }
741
742        assert!(found, "GPS decoding failed!");
743    }
744
745    #[test]
746    fn test_eph2_frame() {
747        #[cfg(all(feature = "std", feature = "log"))]
748        init_logger();
749
750        let mut found = false;
751
752        let bytes = from_ublox_be_bytes(&[
753            // TLM
754            0x22, 0xC1, 0x3E, 0x1B, // HOW
755            0x15, 0x27, 0xEA, 0x1B, // WORD3
756            0x12, 0x7F, 0xF1, 0x65, //WORD4
757            0x8C, 0x68, 0x1F, 0x7C, //WORD5
758            0x02, 0x49, 0x34, 0x15, // WORD6
759            0xBF, 0xF8, 0x81, 0x1E, //WORD7
760            0x99, 0x1B, 0x81, 0x14, // W0RD8
761            0x04, 0x3E, 0x68, 0x6E, // WORD9
762            0x83, 0x34, 0x72, 0x21, // WORD10
763            0x90, 0x42, 0x9F, 0x7B,
764        ]);
765
766        let mut decoder = GpsQzssDecoder::default().without_parity_verification();
767
768        for byte in bytes {
769            if let Some(frame) = decoder.parse(byte) {
770                assert_eq!(decoder.tlm.message, 0x13E);
771                assert_eq!(decoder.tlm.integrity, false);
772                assert_eq!(decoder.tlm.reserved_bits, false);
773
774                assert_eq!(decoder.how.alert, false);
775                assert_eq!(decoder.how.anti_spoofing, true);
776                assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris2);
777
778                match frame.subframe {
779                    GpsQzssSubframe::Ephemeris2(frame2) => {
780                        assert_eq!(frame2.toe, 266_400);
781                        assert_eq!(frame2.crs, -1.843750000000e+000);
782                        assert!((frame2.sqrt_a - 5.153602432251e+003).abs() < 1e-9);
783                        assert!((frame2.m0 - 9.768415465951e-001).abs() < 1e-9);
784                        assert!((frame2.cuc - -5.587935447693e-008).abs() < 1e-9);
785                        assert!((frame2.e - 8.578718174249e-003).abs() < 1e-9);
786                        assert!((frame2.cus - 8.093193173409e-006).abs() < 1e-9);
787                        assert!((frame2.cuc - -5.587935447693e-008).abs() < 1e-6);
788                        assert!((frame2.dn - 1.444277586415e-009).abs() < 1e-9);
789                        assert_eq!(frame2.fit_int_flag, false);
790                        found = true;
791                    },
792                    _ => panic!("incorrect subframe decoded!"),
793                }
794            }
795        }
796
797        assert!(found, "GPS decoding failed!");
798    }
799
800    #[test]
801    fn test_eph3_frame() {
802        #[cfg(all(feature = "std", feature = "log"))]
803        init_logger();
804
805        let mut found = false;
806
807        let bytes = from_ublox_be_bytes(&[
808            0x22, 0xC1, 0x3E, 0x1B, 0x15, 0x28, 0x0B, 0xDB, 0x00, 0x0A, 0xEA, 0x34, 0x03, 0x3C,
809            0xFF, 0xEE, 0xBF, 0xE5, 0xC9, 0xEB, 0x13, 0x6F, 0xB6, 0x4E, 0x86, 0xF4, 0xAB, 0x2C,
810            0x06, 0x71, 0xEB, 0x44, 0x3F, 0xEA, 0xF6, 0x02, 0x92, 0x45, 0x52, 0x13,
811        ]);
812
813        let mut decoder = GpsQzssDecoder::default().without_parity_verification();
814
815        for byte in bytes {
816            if let Some(frame) = decoder.parse(byte) {
817                assert_eq!(decoder.tlm.message, 0x13E);
818                assert_eq!(decoder.tlm.integrity, false);
819                assert_eq!(decoder.tlm.reserved_bits, false);
820
821                assert_eq!(decoder.how.alert, false);
822                assert_eq!(decoder.how.anti_spoofing, true);
823                assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris3);
824
825                match frame.subframe {
826                    GpsQzssSubframe::Ephemeris3(frame3) => {
827                        assert!((frame3.cic - 8.009374141693e-008).abs() < 1e-9);
828                        assert!((frame3.cis - -1.955777406693e-007).abs() < 1E-9);
829                        assert!((frame3.crc - 2.225625000000e+002).abs() < 1E-9);
830                        assert!((frame3.i0 - 3.070601043291e-001).abs() < 1e-9);
831                        assert!((frame3.idot - 1.548414729768e-010).abs() < 1E-9);
832                        assert!((frame3.omega0 - -6.871047024615e-001).abs() < 1e-9);
833                        assert!((frame3.omega_dot - -2.449269231874e-009).abs() < 1e-9);
834                        assert!((frame3.omega - -6.554632573389e-001).abs() < 1e-9);
835                        found = true;
836                    },
837                    _ => panic!("incorrect subframe decoded!"),
838                }
839            }
840        }
841
842        assert!(found, "GPS decoding failed!");
843    }
844}
gnss_protos/gps/decoder.rs

gnss_protos/gps/
decoder.rs
