@@ -469,122 +469,6 @@ void printTimeStamp()
469
469
}
470
470
}
471
471
472
- // Parse the RTCM transport data
473
- // Called by processRTCM in Base.ino - defines whether the data is passed to the NTRIP server
474
- bool checkRtcmMessage (uint8_t data)
475
- {
476
- static uint16_t bytesRemaining;
477
- static uint16_t length;
478
- static uint16_t message;
479
- static bool sendMessage = false ;
480
-
481
- // RTCM Standard 10403.2 - Chapter 4, Transport Layer
482
- //
483
- // |<------------- 3 bytes ------------>|<----- length ----->|<- 3 bytes ->|
484
- // | | | |
485
- // +----------+--------+----------------+---------+----------+-------------+
486
- // | Preamble | Fill | Message Length | Message | Fill | CRC-24Q |
487
- // | 8 bits | 6 bits | 10 bits | n-bits | 0-7 bits | 24 bits |
488
- // | 0xd3 | 000000 | (in bytes) | | zeros | |
489
- // +----------+--------+----------------+---------+----------+-------------+
490
- // | |
491
- // |<------------------------ CRC -------------------------->|
492
- //
493
-
494
- switch (rtcmParsingState)
495
- {
496
- // Read the upper two bits of the length
497
- case RTCM_TRANSPORT_STATE_READ_LENGTH_1:
498
- // Verify the length byte - check the 6 MS bits are all zero
499
- if (!(data & (~3 )))
500
- {
501
- length = data << 8 ;
502
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_LENGTH_2;
503
- break ;
504
- }
505
-
506
- // Wait for the preamble byte
507
- rtcmParsingState = RTCM_TRANSPORT_STATE_WAIT_FOR_PREAMBLE_D3;
508
-
509
- // Fall through
510
- // |
511
- // |
512
- // V
513
-
514
- // Wait for the preamble byte (0xd3)
515
- case RTCM_TRANSPORT_STATE_WAIT_FOR_PREAMBLE_D3:
516
- sendMessage = false ;
517
- if (data == 0xd3 )
518
- {
519
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_LENGTH_1;
520
- sendMessage = true ;
521
- }
522
- break ;
523
-
524
- // Read the lower 8 bits of the length
525
- case RTCM_TRANSPORT_STATE_READ_LENGTH_2:
526
- length |= data;
527
- bytesRemaining = length;
528
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_MESSAGE_1;
529
- break ;
530
-
531
- // Read the upper 8 bits of the message number
532
- case RTCM_TRANSPORT_STATE_READ_MESSAGE_1:
533
- message = data << 4 ;
534
- bytesRemaining -= 1 ;
535
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_MESSAGE_2;
536
- break ;
537
-
538
- // Read the lower 4 bits of the message number
539
- case RTCM_TRANSPORT_STATE_READ_MESSAGE_2:
540
- message |= data >> 4 ;
541
- bytesRemaining -= 1 ;
542
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_DATA;
543
- break ;
544
-
545
- // Read the rest of the message
546
- case RTCM_TRANSPORT_STATE_READ_DATA:
547
- bytesRemaining -= 1 ;
548
- if (bytesRemaining <= 0 )
549
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_CRC_1;
550
- break ;
551
-
552
- // Read the upper 8 bits of the CRC
553
- case RTCM_TRANSPORT_STATE_READ_CRC_1:
554
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_CRC_2;
555
- break ;
556
-
557
- // Read the middle 8 bits of the CRC
558
- case RTCM_TRANSPORT_STATE_READ_CRC_2:
559
- rtcmParsingState = RTCM_TRANSPORT_STATE_READ_CRC_3;
560
- break ;
561
-
562
- // Read the lower 8 bits of the CRC
563
- case RTCM_TRANSPORT_STATE_READ_CRC_3:
564
- rtcmParsingState = RTCM_TRANSPORT_STATE_CHECK_CRC;
565
- break ;
566
-
567
- case RTCM_TRANSPORT_STATE_CHECK_CRC:
568
- break ; // Do nothing. Code below resets the state to PREAMBLE_D3
569
- }
570
-
571
- // Check the CRC. Note: this doesn't actually check the CRC!
572
- if (rtcmParsingState == RTCM_TRANSPORT_STATE_CHECK_CRC)
573
- {
574
- rtcmParsingState = RTCM_TRANSPORT_STATE_WAIT_FOR_PREAMBLE_D3;
575
-
576
- // Display the RTCM message header
577
- if (settings.debugNtripServerRtcm && (!inMainMenu))
578
- {
579
- printTimeStamp ();
580
- systemPrintf (" Tx RTCM %d, %2d bytes\r\n " , message, 3 + length + 3 );
581
- }
582
- }
583
-
584
- // Let the upper layer know if this message should be sent
585
- return sendMessage;
586
- }
587
-
588
472
const double WGS84_A = 6378137 ; // https://geographiclib.sourceforge.io/html/Constants_8hpp_source.html
589
473
const double WGS84_E = 0.081819190842622 ; // http://docs.ros.org/en/hydro/api/gps_common/html/namespacegps__common.html
590
474
// and https://gist.github.com/uhho/63750c4b54c7f90f37f958cc8af0c718
0 commit comments