Merge pull request #50 from sparkfun/pcUpdates

v1.0.4

Author: PaulZC

examples/RTCM_Test/RTCM_Test.ino

@@ -125,9 +125,18 @@ void processMessage(SEMP_PARSE_STATE *parse, uint16_t type)
     // Display the raw message
     Serial.println();
     offset = dataOffset + 1 - parse->length;
-    Serial.printf("Valid RTCM message: %d bytes at 0x%08x (%d)\r\n",
-                  parse->length, offset, offset);
+    Serial.printf("Valid RTCM %d message: %d bytes at 0x%08x (%d)\r\n",
+                  sempRtcmGetMessageNumber(parse), (int)parse->length, (unsigned int)offset, (int)offset);
     dumpBuffer(parse->buffer, parse->length);
+    Serial.printf("Using sempRtcmGetUnsignedBits: message number is: %lld\r\n",
+                  sempRtcmGetUnsignedBits(parse, 0, 12));
+    if (sempRtcmGetMessageNumber(parse) == 1005)
+    {
+        Serial.printf("RTCM 1005 ARP is: X %lld Y %lld Z %lld\r\n",
+                      sempRtcmGetSignedBits(parse, 34, 38),
+                      sempRtcmGetSignedBits(parse, 74, 38),
+                      sempRtcmGetSignedBits(parse, 114, 38));
+    }
 
     // Display the parser state
     if (displayOnce)
@@ -156,7 +165,7 @@ void dumpBuffer(const uint8_t *buffer, uint16_t length)
             bytes = 16 - (offset & 0xf);
 
         // Display the offset
-        Serial.printf("0x%08lx: ", offset);
+        Serial.printf("0x%08x: ", offset);
 
         // Skip leading bytes
         for (index = 0; index < (offset & 0xf); index++)

keywords.txt

@@ -0,0 +1,62 @@
+#######################################
+# Syntax Coloring Map
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+SEMP_PARSE_STATE	KEYWORD1
+SEMP_SCRATCH_PAD	KEYWORD1
+SEMP_PARSE_STATE	KEYWORD1
+SEMP_PARSE_ROUTINE	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+begin	KEYWORD2
+sempAsciiToNibble	KEYWORD2
+sempGetTypeName	KEYWORD2
+sempPrintParserConfiguration	KEYWORD2
+sempGetStateName	KEYWORD2
+sempDisableDebugOutput	KEYWORD2
+sempEnableDebugOutput	KEYWORD2
+sempDisableErrorOutput	KEYWORD2
+sempEnableErrorOutput	KEYWORD2
+sempBeginParser	KEYWORD2
+sempFirstByte	KEYWORD2
+sempParseNextByte	KEYWORD2
+sempParseNextBytes	KEYWORD2
+sempStopParser	KEYWORD2
+sempNmeaGetSentenceName	KEYWORD2
+sempRtcmGetMessageNumber	KEYWORD2
+sempRtcmGetUnsignedBits	KEYWORD2
+sempRtcmGetSignedBits	KEYWORD2
+sempUbloxGetMessageNumber	KEYWORD2
+sempUnicoreHashGetSentenceName	KEYWORD2
+sempSpartnGetMessageType	KEYWORD2
+sempSbfSetInvalidDataCallback	KEYWORD2
+sempSbfGetBlockNumber	KEYWORD2
+sempSbfGetBlockRevision	KEYWORD2
+sempSbfGetU1	KEYWORD2
+sempSbfGetU2	KEYWORD2
+sempSbfGetU4	KEYWORD2
+sempSbfGetU8	KEYWORD2
+sempSbfGetI1	KEYWORD2
+sempSbfGetI2	KEYWORD2
+sempSbfGetI4	KEYWORD2
+sempSbfGetI8	KEYWORD2
+sempSbfGetF4	KEYWORD2
+sempSbfGetF8	KEYWORD2
+sempSbfGetString	KEYWORD2
+sempSbfIsEncapsulatedNMEA	KEYWORD2
+sempSbfIsEncapsulatedRTCMv3	KEYWORD2
+sempSbfGetEncapsulatedPayloadLength	KEYWORD2
+sempSbfGetEncapsulatedPayload	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
+SEMP_MINIMUM_BUFFER_LENGTH	LITERAL1

library.properties

@@ -1,5 +1,5 @@
 name=SparkFun Extensible Message Parser
-version=1.0.3
+version=1.0.4
 author=SparkFun Electronics
 maintainer=SparkFun Electronics <sparkfun.com>
 sentence=Library to parse structured serial streams

src/Parse_RTCM.cpp

@@ -190,3 +190,121 @@ uint16_t sempRtcmGetMessageNumber(const SEMP_PARSE_STATE *parse)
     SEMP_SCRATCH_PAD *scratchPad = (SEMP_SCRATCH_PAD *)parse->scratchPad;
     return scratchPad->rtcm.message;
 }
+
+// Get unsigned integer with width bits, starting at bit start
+uint64_t sempRtcmGetUnsignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width)
+{
+    uint8_t *ptr = parse->buffer;
+    ptr += 3; // Skip the preamble and length bytes
+
+    uint64_t result = 0;
+    uint16_t count = 0;
+    uint8_t bitMask = 0x80;
+
+    // Skip whole bytes (8 bits)
+    ptr += start / 8;
+    count += (start / 8) * 8;
+
+    // Loop until we reach the start bit
+    while (count < start)
+    {
+        bitMask >>= 1; // Shift the bit mask
+        count++;       // Increment the count
+
+        if (bitMask == 0) // Have we counted 8 bits?
+        {
+            ptr++;          // Point to the next byte
+            bitMask = 0x80; // Reset the bit mask
+        }
+    }
+
+    // We have reached the start bit and ptr is pointing at the correct byte
+    // Now extract width bits, incrementing ptr and shifting bitMask as we go
+    while (count < (start + width))
+    {
+        if (*ptr & bitMask) // Is the bit set?
+            result |= 1;      // Set the corresponding bit in result
+
+        bitMask >>= 1; // Shift the bit mask
+        count++;       // Increment the count
+
+        if (bitMask == 0) // Have we counted 8 bits?
+        {
+            ptr++;          // Point to the next byte
+            bitMask = 0x80; // Reset the bit mask
+        }
+
+        if (count < (start + width)) // Do we need to shift result?
+            result <<= 1;              // Shift the result
+    }
+
+    return result;
+}
+
+// Get signed integer with width bits, starting at bit start
+int64_t sempRtcmGetSignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width)
+{
+    uint8_t *ptr = parse->buffer;
+    ptr += 3; // Skip the preamble and length bytes
+
+    union
+    {
+        uint64_t unsigned64;
+        int64_t signed64;
+    } result;
+
+    result.unsigned64 = 0;
+
+    uint64_t twosComplement = 0xFFFFFFFFFFFFFFFF;
+
+    bool isNegative;
+
+    uint16_t count = 0;
+    uint8_t bitMask = 0x80;
+
+    // Skip whole bytes (8 bits)
+    ptr += start / 8;
+    count += (start / 8) * 8;
+
+    // Loop until we reach the start bit
+    while (count < start)
+    {
+        bitMask >>= 1; // Shift the bit mask
+        count++;       // Increment the count
+
+        if (bitMask == 0) // Have we counted 8 bits?
+        {
+            ptr++;          // Point to the next byte
+            bitMask = 0x80; // Reset the bit mask
+        }
+    }
+
+    isNegative = *ptr & bitMask; // Record the first bit - indicates in the number is negative
+
+    // We have reached the start bit and ptr is pointing at the correct byte
+    // Now extract width bits, incrementing ptr and shifting bitMask as we go
+    while (count < (start + width))
+    {
+        if (*ptr & bitMask)       // Is the bit set?
+            result.unsigned64 |= 1; // Set the corresponding bit in result
+
+        bitMask >>= 1;        // Shift the bit mask
+        count++;              // Increment the count
+        twosComplement <<= 1; // Shift the two's complement mask (clear LSB)
+
+        if (bitMask == 0) // Have we counted 8 bits?
+        {
+            ptr++;          // Point to the next byte
+            bitMask = 0x80; // Reset the bit mask
+        }
+
+        if (count < (start + width)) // Do we need to shift result?
+            result.unsigned64 <<= 1;   // Shift the result
+    }
+
+    // Handle negative number
+    if (isNegative)
+        result.unsigned64 |= twosComplement; // OR in the two's complement mask
+
+    return result.signed64;
+}

src/SparkFun_Extensible_Message_Parser.cpp

@@ -373,6 +373,13 @@ void sempParseNextByte(SEMP_PARSE_STATE *parse, uint8_t data)
     }
 }
 
+// Parse the next bytes
+void sempParseNextBytes(SEMP_PARSE_STATE *parse, uint8_t *data, uint16_t len)
+{
+    for (uint16_t i = 0; i < len; i++)
+        sempParseNextByte(parse, *(data++));
+}
+
 // Shutdown the parser
 void sempStopParser(SEMP_PARSE_STATE **parse)
 {

src/SparkFun_Extensible_Message_Parser.h

@@ -288,6 +288,10 @@ bool sempFirstByte(SEMP_PARSE_STATE *parse, uint8_t data);
 // from a raw data stream.
 void sempParseNextByte(SEMP_PARSE_STATE *parse, uint8_t data);
 
+// The routine sempParseNextBytes is used to parse the next bytes
+// from a raw data stream.
+void sempParseNextBytes(SEMP_PARSE_STATE *parse, uint8_t *data, uint16_t len);
+
 // The routine sempStopParser frees the parse data structure and sets
 // the pointer value to nullptr to prevent future references to the
 // freed structure.
@@ -333,6 +337,8 @@ const char * sempNmeaGetSentenceName(const SEMP_PARSE_STATE *parse);
 bool sempRtcmPreamble(SEMP_PARSE_STATE *parse, uint8_t data);
 const char * sempRtcmGetStateName(const SEMP_PARSE_STATE *parse);
 uint16_t sempRtcmGetMessageNumber(const SEMP_PARSE_STATE *parse);
+uint64_t sempRtcmGetUnsignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width);
+int64_t sempRtcmGetSignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width);
 
 // u-blox parse routines
 bool sempUbloxPreamble(SEMP_PARSE_STATE *parse, uint8_t data);