Skip to content

Commit 5cd2d72

Browse files
PaulZCPaulZC
authored
Merge pull request #50 from sparkfun/pcUpdates
v1.0.4
2 parents fe651b1 + 800bf28 commit 5cd2d72

File tree

6 files changed

+206
-4
lines changed

6 files changed

+206
-4
lines changed

‎examples/RTCM_Test/RTCM_Test.ino

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -125,9 +125,18 @@ void processMessage(SEMP_PARSE_STATE *parse, uint16_t type)
125125
// Display the raw message
126126
Serial.println();
127127
offset = dataOffset + 1 - parse->length;
128-
Serial.printf("Valid RTCM message: %d bytes at 0x%08x (%d)\r\n",
129-
parse->length, offset, offset);
128+
Serial.printf("Valid RTCM %d message: %d bytes at 0x%08x (%d)\r\n",
129+
sempRtcmGetMessageNumber(parse), (int)parse->length, (unsigned int)offset, (int)offset);
130130
dumpBuffer(parse->buffer, parse->length);
131+
Serial.printf("Using sempRtcmGetUnsignedBits: message number is: %lld\r\n",
132+
sempRtcmGetUnsignedBits(parse, 0, 12));
133+
if (sempRtcmGetMessageNumber(parse) == 1005)
134+
{
135+
Serial.printf("RTCM 1005 ARP is: X %lld Y %lld Z %lld\r\n",
136+
sempRtcmGetSignedBits(parse, 34, 38),
137+
sempRtcmGetSignedBits(parse, 74, 38),
138+
sempRtcmGetSignedBits(parse, 114, 38));
139+
}
131140

132141
// Display the parser state
133142
if (displayOnce)
@@ -156,7 +165,7 @@ void dumpBuffer(const uint8_t *buffer, uint16_t length)
156165
bytes = 16 - (offset & 0xf);
157166

158167
// Display the offset
159-
Serial.printf("0x%08lx: ", offset);
168+
Serial.printf("0x%08x: ", offset);
160169

161170
// Skip leading bytes
162171
for (index = 0; index < (offset & 0xf); index++)

‎keywords.txt

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
#######################################
2+
# Syntax Coloring Map
3+
#######################################
4+
5+
#######################################
6+
# Datatypes (KEYWORD1)
7+
#######################################
8+
9+
SEMP_PARSE_STATE KEYWORD1
10+
SEMP_SCRATCH_PAD KEYWORD1
11+
SEMP_PARSE_STATE KEYWORD1
12+
SEMP_PARSE_ROUTINE KEYWORD1
13+
14+
#######################################
15+
# Methods and Functions (KEYWORD2)
16+
#######################################
17+
18+
begin KEYWORD2
19+
sempAsciiToNibble KEYWORD2
20+
sempGetTypeName KEYWORD2
21+
sempPrintParserConfiguration KEYWORD2
22+
sempGetStateName KEYWORD2
23+
sempDisableDebugOutput KEYWORD2
24+
sempEnableDebugOutput KEYWORD2
25+
sempDisableErrorOutput KEYWORD2
26+
sempEnableErrorOutput KEYWORD2
27+
sempBeginParser KEYWORD2
28+
sempFirstByte KEYWORD2
29+
sempParseNextByte KEYWORD2
30+
sempParseNextBytes KEYWORD2
31+
sempStopParser KEYWORD2
32+
sempNmeaGetSentenceName KEYWORD2
33+
sempRtcmGetMessageNumber KEYWORD2
34+
sempRtcmGetUnsignedBits KEYWORD2
35+
sempRtcmGetSignedBits KEYWORD2
36+
sempUbloxGetMessageNumber KEYWORD2
37+
sempUnicoreHashGetSentenceName KEYWORD2
38+
sempSpartnGetMessageType KEYWORD2
39+
sempSbfSetInvalidDataCallback KEYWORD2
40+
sempSbfGetBlockNumber KEYWORD2
41+
sempSbfGetBlockRevision KEYWORD2
42+
sempSbfGetU1 KEYWORD2
43+
sempSbfGetU2 KEYWORD2
44+
sempSbfGetU4 KEYWORD2
45+
sempSbfGetU8 KEYWORD2
46+
sempSbfGetI1 KEYWORD2
47+
sempSbfGetI2 KEYWORD2
48+
sempSbfGetI4 KEYWORD2
49+
sempSbfGetI8 KEYWORD2
50+
sempSbfGetF4 KEYWORD2
51+
sempSbfGetF8 KEYWORD2
52+
sempSbfGetString KEYWORD2
53+
sempSbfIsEncapsulatedNMEA KEYWORD2
54+
sempSbfIsEncapsulatedRTCMv3 KEYWORD2
55+
sempSbfGetEncapsulatedPayloadLength KEYWORD2
56+
sempSbfGetEncapsulatedPayload KEYWORD2
57+
58+
#######################################
59+
# Constants (LITERAL1)
60+
#######################################
61+
62+
SEMP_MINIMUM_BUFFER_LENGTH LITERAL1

‎library.properties

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
name=SparkFun Extensible Message Parser
2-
version=1.0.3
2+
version=1.0.4
33
author=SparkFun Electronics
44
maintainer=SparkFun Electronics <sparkfun.com>
55
sentence=Library to parse structured serial streams

‎src/Parse_RTCM.cpp

Lines changed: 118 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -190,3 +190,121 @@ uint16_t sempRtcmGetMessageNumber(const SEMP_PARSE_STATE *parse)
190190
SEMP_SCRATCH_PAD *scratchPad = (SEMP_SCRATCH_PAD *)parse->scratchPad;
191191
return scratchPad->rtcm.message;
192192
}
193+
194+
// Get unsigned integer with width bits, starting at bit start
195+
uint64_t sempRtcmGetUnsignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width)
196+
{
197+
uint8_t *ptr = parse->buffer;
198+
ptr += 3; // Skip the preamble and length bytes
199+
200+
uint64_t result = 0;
201+
uint16_t count = 0;
202+
uint8_t bitMask = 0x80;
203+
204+
// Skip whole bytes (8 bits)
205+
ptr += start / 8;
206+
count += (start / 8) * 8;
207+
208+
// Loop until we reach the start bit
209+
while (count < start)
210+
{
211+
bitMask >>= 1; // Shift the bit mask
212+
count++; // Increment the count
213+
214+
if (bitMask == 0) // Have we counted 8 bits?
215+
{
216+
ptr++; // Point to the next byte
217+
bitMask = 0x80; // Reset the bit mask
218+
}
219+
}
220+
221+
// We have reached the start bit and ptr is pointing at the correct byte
222+
// Now extract width bits, incrementing ptr and shifting bitMask as we go
223+
while (count < (start + width))
224+
{
225+
if (*ptr & bitMask) // Is the bit set?
226+
result |= 1; // Set the corresponding bit in result
227+
228+
bitMask >>= 1; // Shift the bit mask
229+
count++; // Increment the count
230+
231+
if (bitMask == 0) // Have we counted 8 bits?
232+
{
233+
ptr++; // Point to the next byte
234+
bitMask = 0x80; // Reset the bit mask
235+
}
236+
237+
if (count < (start + width)) // Do we need to shift result?
238+
result <<= 1; // Shift the result
239+
}
240+
241+
return result;
242+
}
243+
244+
// Get signed integer with width bits, starting at bit start
245+
int64_t sempRtcmGetSignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width)
246+
{
247+
uint8_t *ptr = parse->buffer;
248+
ptr += 3; // Skip the preamble and length bytes
249+
250+
union
251+
{
252+
uint64_t unsigned64;
253+
int64_t signed64;
254+
} result;
255+
256+
result.unsigned64 = 0;
257+
258+
uint64_t twosComplement = 0xFFFFFFFFFFFFFFFF;
259+
260+
bool isNegative;
261+
262+
uint16_t count = 0;
263+
uint8_t bitMask = 0x80;
264+
265+
// Skip whole bytes (8 bits)
266+
ptr += start / 8;
267+
count += (start / 8) * 8;
268+
269+
// Loop until we reach the start bit
270+
while (count < start)
271+
{
272+
bitMask >>= 1; // Shift the bit mask
273+
count++; // Increment the count
274+
275+
if (bitMask == 0) // Have we counted 8 bits?
276+
{
277+
ptr++; // Point to the next byte
278+
bitMask = 0x80; // Reset the bit mask
279+
}
280+
}
281+
282+
isNegative = *ptr & bitMask; // Record the first bit - indicates in the number is negative
283+
284+
// We have reached the start bit and ptr is pointing at the correct byte
285+
// Now extract width bits, incrementing ptr and shifting bitMask as we go
286+
while (count < (start + width))
287+
{
288+
if (*ptr & bitMask) // Is the bit set?
289+
result.unsigned64 |= 1; // Set the corresponding bit in result
290+
291+
bitMask >>= 1; // Shift the bit mask
292+
count++; // Increment the count
293+
twosComplement <<= 1; // Shift the two's complement mask (clear LSB)
294+
295+
if (bitMask == 0) // Have we counted 8 bits?
296+
{
297+
ptr++; // Point to the next byte
298+
bitMask = 0x80; // Reset the bit mask
299+
}
300+
301+
if (count < (start + width)) // Do we need to shift result?
302+
result.unsigned64 <<= 1; // Shift the result
303+
}
304+
305+
// Handle negative number
306+
if (isNegative)
307+
result.unsigned64 |= twosComplement; // OR in the two's complement mask
308+
309+
return result.signed64;
310+
}

‎src/SparkFun_Extensible_Message_Parser.cpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -373,6 +373,13 @@ void sempParseNextByte(SEMP_PARSE_STATE *parse, uint8_t data)
373373
}
374374
}
375375

376+
// Parse the next bytes
377+
void sempParseNextBytes(SEMP_PARSE_STATE *parse, uint8_t *data, uint16_t len)
378+
{
379+
for (uint16_t i = 0; i < len; i++)
380+
sempParseNextByte(parse, *(data++));
381+
}
382+
376383
// Shutdown the parser
377384
void sempStopParser(SEMP_PARSE_STATE **parse)
378385
{

‎src/SparkFun_Extensible_Message_Parser.h

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -288,6 +288,10 @@ bool sempFirstByte(SEMP_PARSE_STATE *parse, uint8_t data);
288288
// from a raw data stream.
289289
void sempParseNextByte(SEMP_PARSE_STATE *parse, uint8_t data);
290290

291+
// The routine sempParseNextBytes is used to parse the next bytes
292+
// from a raw data stream.
293+
void sempParseNextBytes(SEMP_PARSE_STATE *parse, uint8_t *data, uint16_t len);
294+
291295
// The routine sempStopParser frees the parse data structure and sets
292296
// the pointer value to nullptr to prevent future references to the
293297
// freed structure.
@@ -333,6 +337,8 @@ const char * sempNmeaGetSentenceName(const SEMP_PARSE_STATE *parse);
333337
bool sempRtcmPreamble(SEMP_PARSE_STATE *parse, uint8_t data);
334338
const char * sempRtcmGetStateName(const SEMP_PARSE_STATE *parse);
335339
uint16_t sempRtcmGetMessageNumber(const SEMP_PARSE_STATE *parse);
340+
uint64_t sempRtcmGetUnsignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width);
341+
int64_t sempRtcmGetSignedBits(const SEMP_PARSE_STATE *parse, uint16_t start, uint16_t width);
336342

337343
// u-blox parse routines
338344
bool sempUbloxPreamble(SEMP_PARSE_STATE *parse, uint8_t data);

0 commit comments

Comments
 (0)