Example 5: Integrate the SparkFun Extensible Message Parser

examples/Example5_EnableRTCM/Example5_EnableRTCM.ino

@@ -21,11 +21,34 @@
   Connect a dual or triband GNSS antenna: https://www.sparkfun.com/products/21801
 */
 
-int pin_UART1_TX = 4;
-int pin_UART1_RX = 13;
+int pin_UART_TX = 4;
+int pin_UART_RX = 13;
 
 #include <SparkFun_Unicore_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_Unicore_GNSS
+#include <SparkFun_Extensible_Message_Parser.h> //http://librarymanager/All#SparkFun_Extensible_Message_Parser
 
+//----------------------------------------
+// Constants
+//----------------------------------------
+
+// Build the table listing all of the parsers
+SEMP_PARSE_ROUTINE const parserTable[] =
+{
+    sempRtcmPreamble
+};
+const int parserCount = sizeof(parserTable) / sizeof(parserTable[0]);
+
+const char * const parserNames[] =
+{
+    "RTCM parser"
+};
+const int parserNameCount = sizeof(parserNames) / sizeof(parserNames[0]);
+
+//----------------------------------------
+// Locals
+//----------------------------------------
+
+SEMP_PARSE_STATE *parse;
 UM980 myGNSS;
 
 HardwareSerial SerialGNSS(1); //Use UART1 on the ESP32
@@ -37,8 +60,15 @@ void setup()
   Serial.println();
   Serial.println("SparkFun UM980 Example");
 
+  // Initialize the parser
+  parse = sempInitParser(parserTable, parserCount,
+                         parserNames, parserNameCount,
+                         0, 3000, processMessage, "RTCM_Test");
+  if (!parse)
+    reportFatalError("Failed to initialize the parser");
+
   //We must start the serial port before using it in the library
-  SerialGNSS.begin(115200, SERIAL_8N1, pin_UART1_RX, pin_UART1_TX);
+  SerialGNSS.begin(115200, SERIAL_8N1, pin_UART_RX, pin_UART_TX);
 
   myGNSS.enableDebugging(); // Print all debug to Serial
 
@@ -62,11 +92,99 @@ void setup()
 
   myGNSS.saveConfiguration(); //Save the current configuration into non-volatile memory (NVM)
 
-  Serial.println("RTCM is in binary so printing it will show random characters");
+  Serial.println("RTCM messages are dumped in HEX if the CRC is correct");
 }
 
 void loop()
 {
   while (SerialGNSS.available())
-    Serial.write(SerialGNSS.read());
-}
+    // Update the parser state based on the incoming byte
+    sempParseNextByte(parse, SerialGNSS.read());
+}
+
+// Output a line of text for the SparkFun Extensible Message Parser
+void sempExtPrintLineOfText(const char *string)
+{
+  Serial.println(string);
+}
+
+// Call back from within parser, for end of message
+// Process a complete message incoming from parser
+void processMessage(SEMP_PARSE_STATE *parse, uint8_t type)
+{
+  SEMP_SCRATCH_PAD *scratchPad = (SEMP_SCRATCH_PAD *)parse->scratchPad;
+  static bool displayOnce = true;
+
+  // Display the raw message
+  Serial.println();
+  Serial.printf("Valid RTCM message: 0x%04x (%d) bytes\r\n", parse->length, parse->length);
+  ex5DumpBuffer(parse->buffer, parse->length);
+
+  // Display the parser state
+  if (displayOnce)
+  {
+    displayOnce = false;
+    Serial.println();
+    sempPrintParserConfiguration(parse);
+  }
+}
+
+// Display the contents of a buffer
+void ex5DumpBuffer(const uint8_t *buffer, uint16_t length)
+{
+  int bytes;
+  const uint8_t *end;
+  int index;
+  uint16_t offset;
+
+  end = &buffer[length];
+  offset = 0;
+  while (buffer < end)
+  {
+    // Determine the number of bytes to display on the line
+    bytes = end - buffer;
+    if (bytes > (16 - (offset & 0xf)))
+      bytes = 16 - (offset & 0xf);
+
+    // Display the offset
+    Serial.printf("0x%08lx: ", offset);
+
+    // Skip leading bytes
+    for (index = 0; index < (offset & 0xf); index++)
+      Serial.printf("   ");
+
+    // Display the data bytes
+    for (index = 0; index < bytes; index++)
+      Serial.printf("%02x ", buffer[index]);
+
+    // Separate the data bytes from the ASCII
+    for (; index < (16 - (offset & 0xf)); index++)
+      Serial.printf("   ");
+    Serial.printf(" ");
+
+    // Skip leading bytes
+    for (index = 0; index < (offset & 0xf); index++)
+      Serial.printf(" ");
+
+    // Display the ASCII values
+    for (index = 0; index < bytes; index++)
+      Serial.printf("%c", ((buffer[index] < ' ') || (buffer[index] >= 0x7f)) ? '.' : buffer[index]);
+    Serial.printf("\r\n");
+
+    // Set the next line of data
+    buffer += bytes;
+    offset += bytes;
+  }
+}
+
+// Print the error message every 15 seconds
+void reportFatalError(const char *errorMsg)
+{
+  while (1)
+  {
+    Serial.print("HALTED: ");
+    Serial.print(errorMsg);
+    Serial.println();
+    sleep(15);
+  }
+}