1mod checksum;
2mod meta; mod mid; mod record; mod time; pub use record::{
8 EphemerisFrame, GALEphemeris, GLOEphemeris, GPSEphemeris, GPSRaw, GeoStringFrame,
9 MonumentGeoMetadata, MonumentGeoRecord, PositionEcef3d, PositionGeo3d, Record, SBASEphemeris,
10 Solutions, SolutionsFrame, TemporalSolution, Velocity3d, VelocityNED3d,
11};
12
13pub use meta::Meta;
14
15pub(crate) use mid::MessageID;
16
17use crate::{stream::Provider, ClosedSourceMeta, Error};
18use checksum::Checksum;
19
20#[derive(Debug, Clone, PartialEq, Default)]
21pub struct Message {
22 pub meta: Meta,
24 pub record: Record,
26}
27
28impl Message {
29 const BNXI_KEEP_GOING_MASK: u8 = 0x80;
32
33 const BNXI_BYTE_MASK: u8 = 0x7f;
36
37 pub fn new(meta: Meta, record: Record) -> Self {
39 Self { meta, record }
40 }
41
42 pub fn encoding_size(&self) -> usize {
45 let mut total = 1; let mid = self.record.to_message_id() as u32;
48 let mid_1_4 = Self::bnxi_encoding_size(mid);
49 total += mid_1_4;
50
51 let mlen = self.record.encoding_size();
52 let mlen_1_4 = Self::bnxi_encoding_size(mlen as u32);
53 total += mlen_1_4;
54 total += mlen;
55
56 let ck = Checksum::from_len(mlen_1_4 + mlen + mid_1_4, self.meta.enhanced_crc);
57 total += ck.len();
58
59 total
60 }
61
62 pub fn decode(buf: &[u8]) -> Result<Self, Error> {
68 let buf_len = buf.len();
69
70 let meta = Meta::find_and_parse(buf, buf_len);
72 if meta.is_none() {
73 return Err(Error::NoSyncByte);
74 }
75
76 let (meta, sync_off) = meta.unwrap();
77
78 let reversed = meta.reversed;
79 let big_endian = meta.big_endian;
80 let enhanced_crc = meta.enhanced_crc;
81
82 if reversed {
86 return Err(Error::ReversedStream);
88 }
89 if enhanced_crc {
90 return Err(Error::EnhancedCrc);
92 }
93
94 if buf_len - sync_off < 4 {
96 return Err(Error::NotEnoughBytes);
97 }
98
99 let mut ptr = sync_off + 1;
100
101 let (bnxi, mid_1_4) = Self::decode_bnxi(&buf[ptr..], big_endian);
103 let mid = MessageID::from(bnxi);
104
105 if mid == MessageID::Unknown {
106 return Err(Error::UnknownMessage);
107 }
108 ptr += mid_1_4;
109
110 if buf_len - ptr < 4 {
112 return Err(Error::NotEnoughBytes);
113 }
114
115 let (mlen, mlen_1_4) = Self::decode_bnxi(&buf[ptr..], big_endian);
117 let mlen = mlen as usize;
118 if ptr + mlen > buf_len {
121 return Err(Error::IncompleteMessage(mlen));
123 }
124 ptr += mlen_1_4;
125
126 let record = match mid {
128 MessageID::SiteMonumentMarker => {
129 let rec = MonumentGeoRecord::decode(mlen, big_endian, &buf[ptr..])?;
130 Record::new_monument_geo(rec)
131 },
132 MessageID::Ephemeris => {
133 let fr = EphemerisFrame::decode(big_endian, &buf[ptr..])?;
134 Record::new_ephemeris_frame(fr)
135 },
136 MessageID::ProcessedSolutions => {
137 let solutions = Solutions::decode(mlen, big_endian, &buf[ptr..])?;
138 Record::new_solutions(solutions)
139 },
140 MessageID::Unknown => {
141 return Err(Error::UnknownMessage);
142 },
143 _ => {
144 if let Some(provider) = Provider::match_any(mid.into()) {
146 return Err(Error::ClosedSourceMessage(ClosedSourceMeta {
147 mlen,
148 provider,
149 size: mlen,
150 offset: ptr,
151 open_meta: meta,
152 mid: mid.into(),
153 }));
154 } else {
155 return Err(Error::NonSupportedMesssage(mlen));
157 }
158 },
159 };
160
161 let checksum = Checksum::from_len(mlen, enhanced_crc);
163 let ck_len = checksum.len();
164
165 if ptr + mlen + ck_len > buf_len {
166 return Err(Error::MissingCRC);
167 }
168
169 let ck = checksum.decode(&buf[ptr + mlen..], ck_len, big_endian);
171
172 let expected = checksum.calc(&buf[sync_off + 1..], mlen + mid_1_4 + mlen_1_4);
174
175 if expected != ck {
176 Err(Error::CorrupctBadCRC)
177 } else {
178 Ok(Self { meta, record })
179 }
180 }
181
182 pub fn encode(&self, buf: &mut [u8], buf_size: usize) -> Result<usize, Error> {
189 let total = self.encoding_size();
190
191 if buf_size < total {
192 return Err(Error::NotEnoughBytes);
193 }
194
195 let big_endian = self.meta.big_endian;
197 let enhanced_crc = self.meta.enhanced_crc;
199
200 buf[0] = self.meta.sync_byte();
202 let mut ptr = 1;
203
204 let mid = self.record.to_message_id() as u32;
206 let mid_1_4 = Self::encode_bnxi(mid, big_endian, &mut buf[ptr..])?;
207 ptr += mid_1_4;
208
209 let mlen = self.record.encoding_size();
211 let mlen_1_4 = Self::encode_bnxi(mlen as u32, big_endian, &mut buf[ptr..])?;
212 ptr += mlen_1_4;
213
214 match &self.record {
216 Record::EphemerisFrame(fr) => {
217 ptr += fr.encode(big_endian, &mut buf[ptr..])?;
218 },
219 Record::MonumentGeo(geo) => {
220 ptr += geo.encode(big_endian, &mut buf[ptr..])?;
221 },
222 Record::Solutions(fr) => {
223 ptr += fr.encode(big_endian, &mut buf[ptr..])?;
224 },
225 }
226
227 let ck = Checksum::from_len(mlen, enhanced_crc);
229 let ck_len = ck.len();
230 let crc_u128 = ck.calc(&buf[1..], mlen + mid_1_4 + mlen_1_4);
231
232 if ck_len == 1 {
233 buf[ptr] = crc_u128 as u8;
234 } else if ck_len == 2 {
235 let crc_bytes = if big_endian {
236 (crc_u128 as u16).to_be_bytes()
237 } else {
238 (crc_u128 as u16).to_le_bytes()
239 };
240
241 for i in 0..ck_len {
242 buf[ptr + i] = crc_bytes[i];
243 }
244 } else if ck_len == 4 {
245 let crc_bytes = if big_endian {
246 (crc_u128 as u32).to_be_bytes()
247 } else {
248 (crc_u128 as u32).to_le_bytes()
249 };
250 for i in 0..ck_len {
251 buf[ptr + i] = crc_bytes[i];
252 }
253 } else {
254 let crc_bytes = if big_endian {
255 crc_u128.to_be_bytes()
256 } else {
257 crc_u128.to_le_bytes()
258 };
259 for i in 0..ck_len {
260 buf[ptr + i] = crc_bytes[i];
261 }
262 }
263
264 Ok(ptr + ck_len)
265 }
266
267 pub(crate) const fn bnxi_encoding_size(val: u32) -> usize {
269 if val < 128 {
270 1
271 } else if val < 16384 {
272 2
273 } else if val < 2097152 {
274 3
275 } else {
276 4
277 }
278 }
279
280 pub(crate) fn decode_bnxi(buf: &[u8], big_endian: bool) -> (u32, usize) {
288 let min_size = buf.len().min(4);
289
290 if min_size == 0 {
292 return (0, 0);
293 }
294
295 if buf[0] & Self::BNXI_KEEP_GOING_MASK == 0 {
297 let val32 = buf[0] as u32;
298 return (val32 & 0x7f, 1);
299 }
300
301 let (val, size) = if buf[1] & Self::BNXI_KEEP_GOING_MASK == 0 {
303 let mut val;
304
305 let (byte0, byte1) = if big_endian {
306 (buf[0], buf[1])
307 } else {
308 (buf[1], buf[0])
309 };
310
311 val = (byte0 & Self::BNXI_BYTE_MASK) as u32;
312 val <<= 7;
313 val |= byte1 as u32;
314
315 (val, 2)
316 } else if buf[2] & Self::BNXI_KEEP_GOING_MASK == 0 {
317 let mut val;
318
319 let (byte0, byte1, byte2) = if big_endian {
320 (buf[0], buf[1], buf[2])
321 } else {
322 (buf[2], buf[1], buf[0])
323 };
324
325 val = (byte0 & Self::BNXI_BYTE_MASK) as u32;
326 val <<= 8;
327
328 val |= (byte1 & Self::BNXI_BYTE_MASK) as u32;
329 val <<= 7;
330
331 val |= byte2 as u32;
332 (val, 3)
333 } else {
334 let mut val;
335
336 let (byte0, byte1, byte2, byte3) = if big_endian {
337 (buf[0], buf[1], buf[2], buf[3])
338 } else {
339 (buf[3], buf[2], buf[1], buf[0])
340 };
341
342 val = (byte0 & Self::BNXI_BYTE_MASK) as u32;
343 val <<= 8;
344
345 val |= (byte1 & Self::BNXI_BYTE_MASK) as u32;
346 val <<= 8;
347
348 val |= (byte2 & Self::BNXI_BYTE_MASK) as u32;
349 val <<= 7;
350
351 val |= byte3 as u32;
352 (val, 4)
353 };
354
355 (val, size)
356 }
357
358 pub(crate) fn encode_bnxi(val: u32, big_endian: bool, buf: &mut [u8]) -> Result<usize, Error> {
362 let size = Self::bnxi_encoding_size(val);
363 if buf.len() < size {
364 return Err(Error::NotEnoughBytes);
365 }
366
367 if size == 1 {
369 buf[0] = (val as u8) & 0x7f;
370 return Ok(1);
371 }
372
373 let mut val32 = (val & 0xffffff80) << 1;
375 val32 |= val & 0xff;
376
377 if size == 2 {
378 val32 |= 0x8000;
379 val32 &= 0xff7f;
380
381 if big_endian {
382 buf[0] = ((val32 & 0xff00) >> 8) as u8;
383 buf[1] = val32 as u8;
384 } else {
385 buf[1] = ((val32 & 0xff00) >> 8) as u8;
386 buf[0] = val32 as u8;
387 }
388 } else if size == 3 {
389 val32 |= 0x808000;
390 val32 &= 0xffff7f;
391
392 if big_endian {
393 buf[0] = ((val32 & 0xffff00) >> 16) as u8;
394 buf[1] = ((val32 & 0xff00) >> 8) as u8;
395 buf[2] = val32 as u8;
396 } else {
397 buf[2] = ((val32 & 0xffff00) >> 16) as u8;
398 buf[1] = ((val32 & 0xff00) >> 8) as u8;
399 buf[0] = val32 as u8;
400 }
401 } else {
402 val32 |= 0x80808000;
403 val32 &= 0xffffff7f;
404
405 if big_endian {
406 buf[0] = ((val32 & 0xffffff00) >> 24) as u8;
407 buf[1] = ((val32 & 0xffff00) >> 16) as u8;
408 buf[2] = ((val32 & 0xff00) >> 8) as u8;
409 buf[3] = val32 as u8;
410 } else {
411 buf[3] = ((val32 & 0xffffff00) >> 24) as u8;
412 buf[2] = ((val32 & 0xffff00) >> 16) as u8;
413 buf[1] = ((val32 & 0xff00) >> 8) as u8;
414 buf[0] = val32 as u8;
415 }
416 }
417
418 Ok(size)
419 }
420}
421
422#[cfg(test)]
423mod test {
424 use super::Message;
425 use crate::message::{
426 EphemerisFrame, GALEphemeris, GPSEphemeris, GPSRaw, Meta, MonumentGeoMetadata,
427 MonumentGeoRecord, PositionEcef3d, Record, Solutions, SolutionsFrame, Velocity3d,
428 };
429 use crate::prelude::Epoch;
430 use crate::Error;
431
432 #[test]
433 fn big_endian_bnxi() {
434 let buf = [0];
435 let (decoded, size) = Message::decode_bnxi(&buf, true);
436 assert_eq!(size, 1);
437 assert_eq!(decoded, 0);
438
439 let mut encoded = [0; 4];
440 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
441
442 assert_eq!(size, 1);
443 assert_eq!(encoded, [0, 0, 0, 0]);
444
445 let buf = [0, 0, 0, 0];
446 let (decoded, size) = Message::decode_bnxi(&buf, true);
447 assert_eq!(size, 1);
448 assert_eq!(decoded, 0);
449
450 let mut encoded = [0; 4];
451 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
452
453 assert_eq!(size, 1);
454 assert_eq!(encoded, [0, 0, 0, 0]);
455
456 let buf = [1, 0, 0, 0];
457 let (decoded, size) = Message::decode_bnxi(&buf, true);
458 assert_eq!(size, 1);
459 assert_eq!(decoded, 1);
460
461 let mut encoded = [0; 4];
462 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
463
464 assert_eq!(size, 1);
465 assert_eq!(encoded, [1, 0, 0, 0]);
466
467 let buf = [2, 0, 0, 0];
468 let (decoded, size) = Message::decode_bnxi(&buf, true);
469 assert_eq!(size, 1);
470 assert_eq!(decoded, 2);
471
472 let mut encoded = [0; 4];
473 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
474
475 assert_eq!(size, 1);
476 assert_eq!(encoded, [2, 0, 0, 0]);
477
478 let buf = [127, 0, 0, 0];
479 let (decoded, size) = Message::decode_bnxi(&buf, true);
480 assert_eq!(size, 1);
481 assert_eq!(decoded, 127);
482
483 let mut encoded = [0; 4];
484 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
485
486 assert_eq!(size, 1);
487 assert_eq!(encoded, [127, 0, 0, 0]);
488
489 let buf = [129, 0, 0, 0];
490 let (decoded, size) = Message::decode_bnxi(&buf, true);
491 assert_eq!(size, 2);
492 assert_eq!(decoded, 128);
493
494 let mut encoded = [0; 4];
495 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
496
497 assert_eq!(size, 2);
498 assert_eq!(encoded, buf);
499
500 let buf = [0x83, 0x7a, 0, 0];
501 let (decoded, size) = Message::decode_bnxi(&buf, true);
502 assert_eq!(size, 2);
503 assert_eq!(decoded, 0x1fa);
504
505 let mut encoded = [0; 4];
506 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
507
508 assert_eq!(size, 2);
509 assert_eq!(encoded, buf);
510
511 let buf = [0x83, 0x83, 0x7a, 0];
512 let (decoded, size) = Message::decode_bnxi(&buf, true);
513 assert_eq!(size, 3);
514 assert_eq!(decoded, 0x181fa);
515
516 let mut encoded = [0; 4];
517 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
518
519 assert_eq!(size, 3);
520 assert_eq!(encoded, buf);
521
522 let buf = [0x83, 0x83, 0x83, 0x7a];
523 let (decoded, size) = Message::decode_bnxi(&buf, true);
524 assert_eq!(size, 4);
525 assert_eq!(decoded, 0x18181fa);
526
527 let mut encoded = [0; 4];
528 let size = Message::encode_bnxi(decoded, true, &mut encoded).unwrap();
529
530 assert_eq!(size, 4);
531 assert_eq!(encoded, buf);
532 }
533
534 #[test]
535 fn bigend_bnxi_1() {
536 for val in [0, 1, 10, 120, 122, 127] {
537 let mut buf = [0; 1];
538 let size = Message::encode_bnxi(val, true, &mut buf).unwrap();
539
540 assert_eq!(size, 1);
541 assert_eq!(buf[0], val as u8);
542
543 let mut buf = [0; 4];
544
545 let size = Message::encode_bnxi(val, true, &mut buf).unwrap();
546
547 assert_eq!(size, 1);
548
549 assert_eq!(buf[0], val as u8);
550 assert_eq!(buf[1], 0);
551 assert_eq!(buf[2], 0);
552 assert_eq!(buf[3], 0);
553
554 let (decoded, size) = Message::decode_bnxi(&buf, true);
555 assert_eq!(size, 1);
556 assert_eq!(decoded, val);
557 }
558 }
559
560 #[test]
561 fn decode_no_sync_byte() {
562 let buf = [0, 0, 0, 0, 0];
563 match Message::decode(&buf) {
564 Err(Error::NoSyncByte) => {},
565 Err(e) => panic!("returned unexpected error: {:?}", e),
566 _ => panic!("should have paniced"),
567 }
568 let buf = [0, 0, 0, 0, 0];
569 match Message::decode(&buf) {
570 Err(Error::NoSyncByte) => {},
571 Err(e) => panic!("returned unexpected error: {:?}", e),
572 _ => panic!("should have paniced"),
573 }
574 }
575
576 #[test]
577 fn test_monument_geo() {
578 let mut meta = Meta::default();
579
580 meta.reversed = false;
581 meta.big_endian = true;
582 meta.enhanced_crc = false;
583
584 let mut geo = MonumentGeoRecord::default().with_comment("simple");
585
586 geo.epoch = Epoch::from_gpst_seconds(1.0);
587 geo.meta = MonumentGeoMetadata::RNX2BIN;
588
589 let geo_len = geo.encoding_size();
590 let record = Record::new_monument_geo(geo);
591
592 let msg = Message::new(meta, record);
593
594 assert_eq!(msg.encoding_size(), 1 + 1 + 1 + geo_len + 1);
596
597 let mut encoded = [0; 256];
598 msg.encode(&mut encoded, 256).unwrap();
599
600 assert_eq!(encoded[17], 3);
601
602 let parsed = Message::decode(&encoded).unwrap();
604 assert_eq!(parsed, msg);
605 }
606
607 #[test]
608 fn test_gps_raw() {
609 let mut meta = Meta::default();
610
611 meta.reversed = false;
612 meta.big_endian = true;
613 meta.enhanced_crc = false;
614
615 let gps_raw = EphemerisFrame::GPSRaw(GPSRaw::default());
616 let gps_raw_len = gps_raw.encoding_size();
617 let record = Record::new_ephemeris_frame(gps_raw);
618
619 let msg = Message::new(meta, record);
620
621 assert_eq!(msg.encoding_size(), 1 + 1 + 1 + gps_raw_len + 1);
623
624 let mut encoded = [0; 256];
625 msg.encode(&mut encoded, 256).unwrap();
626
627 assert_eq!(encoded[78 + 1 + 1 + 1], 0);
628
629 let parsed = Message::decode(&encoded).unwrap();
631 assert_eq!(parsed, msg);
632 }
633
634 #[test]
635 fn test_gps_eph() {
636 let mut meta = Meta::default();
637
638 meta.reversed = false;
639 meta.big_endian = true;
640 meta.enhanced_crc = false;
641
642 let gps_eph = EphemerisFrame::GPS(GPSEphemeris::default());
643 let gps_eph_len = gps_eph.encoding_size();
644 let record = Record::new_ephemeris_frame(gps_eph);
645
646 assert_eq!(gps_eph_len, 129);
647
648 let msg = Message::new(meta, record);
649
650 assert_eq!(msg.encoding_size(), 1 + 1 + 2 + gps_eph_len + 2);
652
653 let mut encoded = [0; 256];
654 msg.encode(&mut encoded, 256).unwrap();
655
656 let parsed = Message::decode(&encoded).unwrap();
658 assert_eq!(parsed, msg);
659 }
660
661 #[test]
662 fn test_gal_eph() {
663 let mut meta = Meta::default();
664
665 meta.reversed = false;
666 meta.big_endian = true;
667 meta.enhanced_crc = false;
668
669 let eph = EphemerisFrame::GAL(GALEphemeris::default());
670 let eph_len = eph.encoding_size();
671 let record = Record::new_ephemeris_frame(eph);
672
673 assert_eq!(eph_len, 129);
674
675 let msg = Message::new(meta, record);
676
677 assert_eq!(msg.encoding_size(), 1 + 1 + 2 + eph_len + 2);
679
680 let mut encoded = [0; 256];
681 msg.encode(&mut encoded, 256).unwrap();
682
683 let parsed = Message::decode(&encoded).unwrap();
685 assert_eq!(parsed, msg);
686 }
687
688 #[test]
689 fn test_pvt_wgs84() {
690 let mut meta = Meta::default();
691
692 meta.reversed = false;
693 meta.big_endian = true;
694 meta.enhanced_crc = false;
695
696 let mut solutions = Solutions::new(Epoch::from_gpst_seconds(1.100));
697
698 solutions.frames.push(SolutionsFrame::AntennaEcefPosition(
699 PositionEcef3d::new_wgs84(1.0, 2.0, 3.0),
700 ));
701
702 let sol_len = solutions.encoding_size();
703 assert_eq!(sol_len, 6 + 1 + 3 * 8 + 1); let mut buf = [0; 32];
706 let size = solutions.encode(true, &mut buf).unwrap();
707 assert_eq!(size, 6 + 1 + 3 * 8 + 1);
708
709 assert_eq!(
710 buf,
711 [
712 0, 0, 0, 0, 4, 76, 1, 0, 63, 240, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0, 0, 0, 64, 8,
713 0, 0, 0, 0, 0, 0
714 ]
715 );
716
717 let record = Record::new_solutions(solutions.clone());
718 let msg = Message::new(meta, record);
719
720 let mlen = 1 + 1 + 1 + sol_len + 1;
722 assert_eq!(msg.encoding_size(), mlen);
723
724 let mut encoded = [0; 40];
725 msg.encode(&mut encoded, 40).unwrap();
726
727 assert_eq!(
728 encoded,
729 [
730 226, 5, 32, 0, 0, 0, 0, 4, 76, 1, 0, 63, 240, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0,
731 0, 0, 64, 8, 0, 0, 0, 0, 0, 0, 171, 0, 0, 0, 0
732 ]
733 );
734
735 let parsed = Message::decode(&encoded).unwrap();
737 assert_eq!(parsed, msg);
738
739 solutions
741 .frames
742 .push(SolutionsFrame::AntennaEcefVelocity(Velocity3d {
743 x_m_s: 1.0,
744 y_m_s: 1.0,
745 z_m_s: 1.0,
746 }));
747
748 let sol_len = solutions.encoding_size();
749 assert_eq!(sol_len, 6 + 1 + 3 * 8 + 1 + 3 * 8 + 1);
750
751 let mut buf = [0; 64];
752 let size = solutions.encode(true, &mut buf).unwrap();
753 assert_eq!(size, sol_len);
754
755 let record = Record::new_solutions(solutions.clone());
756 let msg = Message::new(meta, record);
757
758 }
763}