Skip to content

Commit 956b41f

Browse files
PaulZCPaulZC
committed
Added most of the packetBuf functionality
1 parent 504efe7 commit 956b41f

File tree

2 files changed

+158
-60
lines changed

2 files changed

+158
-60
lines changed

‎src/SparkFun_Ublox_Arduino_Library.cpp‎

Lines changed: 153 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -127,8 +127,8 @@ const char *SFE_UBLOX_GPS::statusString(sfe_ublox_status_e stat)
127127
case SFE_UBLOX_STATUS_TIMEOUT:
128128
return "Timeout";
129129
break;
130-
case SFE_UBLOX_STATUS_COMMAND_UNKNOWN:
131-
return "Command Unknown (NACK)";
130+
case SFE_UBLOX_STATUS_COMMAND_NACK:
131+
return "Command not acknowledged (NACK)";
132132
break;
133133
case SFE_UBLOX_STATUS_OUT_OF_RANGE:
134134
return "Out of range";
@@ -441,6 +441,12 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re
441441
//We still don't know the response class
442442
ubxFrameCounter = 0;
443443
currentSentence = UBX;
444+
//Reset the packetBuf.counter even though we will need to reset it again when ubxFrameCounter == 2
445+
packetBuf.counter = 0;
446+
//We should not ignore this payload - yet
447+
ignoreThisPayload = false;
448+
//Store data in packetBuf until we know if we have a requested class and ID match
449+
activePacketBuffer = SFE_UBLOX_PACKET_PACKETBUF;
444450
}
445451
else if (incoming == '$')
446452
{
@@ -465,33 +471,140 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re
465471
currentSentence = NONE; //Something went wrong. Reset.
466472
else if ((ubxFrameCounter == 1) && (incoming != 0x62)) //ASCII 'b'
467473
currentSentence = NONE; //Something went wrong. Reset.
468-
else if (ubxFrameCounter == 2) //Class
474+
else if (ubxFrameCounter == 2) //Class
469475
{
476+
// Record the class in packetBuf until we know what to do with it
477+
packetBuf.cls = incoming; // (Duplication)
470478
//Reset our rolling checksums here (not when we receive the 0xB5)
471479
rollingChecksumA = 0;
472480
rollingChecksumB = 0;
481+
//Reset the packetBuf.counter (again)
482+
packetBuf.counter = 0;
483+
}
484+
// Note to future self:
485+
// There may be some duplication / redundancy in the next few lines as processUBX will also
486+
// load information into packetBuf, but we'll do it here too for clarity
487+
else if (ubxFrameCounter == 3) //ID
488+
{
489+
// Record the ID in packetBuf until we know what to do with it
490+
packetBuf.id = incoming; // (Duplication)
473491
//We can now identify the type of response
474-
if (incoming == UBX_CLASS_ACK)
492+
//If the packet we are receiving is not an ACK then check for a class and ID match
493+
if (packetBuf.cls != UBX_CLASS_ACK)
475494
{
476-
packetAck.counter = 0;
477-
packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
478-
ubxFrameClass = CLASS_ACK;
495+
//This is not an ACK so check for a class and ID match
496+
if ((packetBuf.cls == requestedClass) && (packetBuf.id == requestedID))
497+
{
498+
//This is not an ACK and we have a class and ID match
499+
//So start diverting data into incomingUBX (usually packetCfg)
500+
activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG;
501+
//Copy the class and ID into incomingUBX (usually packetCfg)
502+
incomingUBX->cls = packetBuf.cls;
503+
incomingUBX->id = packetBuf.id;
504+
//Copy over the .counter too
505+
incomingUBX->counter = packetBuf.counter;
506+
}
507+
else
508+
{
509+
//This is not an ACK and we do not have a class and ID match
510+
//so we should keep diverting data into packetBuf and ignore the payload
511+
ignoreThisPayload = true;
512+
}
479513
}
480514
else
481515
{
482-
incomingUBX->counter = 0;
483-
incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
484-
ubxFrameClass = CLASS_NOT_AN_ACK;
516+
// This is an ACK so it is to early to do anything with it
517+
// We need to wait until we have received the length and data bytes
518+
// So we should keep diverting data into packetBuf
519+
}
520+
}
521+
else if (ubxFrameCounter == 4) //Length LSB
522+
{
523+
//We should save the length in packetBuf even if activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG
524+
packetBuf.len = incoming; // (Duplication)
525+
}
526+
else if (ubxFrameCounter == 5) //Length MSB
527+
{
528+
//We should save the length in packetBuf even if activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG
529+
packetBuf.len |= incoming << 8; // (Duplication)
530+
}
531+
else if (ubxFrameCounter == 6) //This should be the first byte of the payload unless .len is zero
532+
{
533+
if (packetBuf.len == 0) // Check if length is zero (hopefully this is impossible!)
534+
{
535+
if (_printDebug == true)
536+
{
537+
_debugSerial->print(F("process: ZERO LENGTH packet received: Class: 0x"));
538+
_debugSerial->print(packetBuf.cls, HEX);
539+
_debugSerial->print(F(" ID: 0x"));
540+
_debugSerial->println(packetBuf.id, HEX);
541+
}
542+
//If length is zero (!) this will be the first byte of the checksum so record it
543+
packetBuf.checksumA = incoming;
544+
}
545+
else
546+
{
547+
//The length is not zero so record this byte in the payload
548+
packetBuf.payload[0] = incoming;
549+
}
550+
}
551+
else if (ubxFrameCounter == 7) //This should be the second byte of the payload unless .len is zero or one
552+
{
553+
if (packetBuf.len == 0) // Check if length is zero (hopefully this is impossible!)
554+
{
555+
//If length is zero (!) this will be the second byte of the checksum so record it
556+
packetBuf.checksumB = incoming;
557+
}
558+
else if (packetBuf.len == 1) // Check if length is one
559+
{
560+
//The length is one so this is the first byte of the checksum
561+
packetBuf.checksumA = incoming;
562+
}
563+
else // Length is >= 2 so this must be a payload byte
564+
{
565+
packetBuf.payload[1] = incoming;
566+
}
567+
// Now that we have received two payload bytes, we can check for a matching ACK/NACK
568+
if ((activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF) // If we are not already processing a data packet
569+
&& (packetBuf.cls == UBX_CLASS_ACK) // and if this is an ACK/NACK
570+
&& (packetBuf.payload[0] == requestedClass) // and if the class matches
571+
&& (packetBuf.payload[1] == requestedID)) // and if the ID matches
572+
{
573+
if (packetBuf.len != 2) // Check if length is not 2 (hopefully this is impossible!)
574+
{
575+
if (_printDebug == true)
576+
{
577+
_debugSerial->print(F("process: ACK received with .len != 2: Class: 0x"));
578+
_debugSerial->print(packetBuf.payload[0], HEX);
579+
_debugSerial->print(F(" ID: 0x"));
580+
_debugSerial->println(packetBuf.payload[1], HEX);
581+
}
582+
}
583+
else
584+
{
585+
// Then this is a matching ACK so copy it into packetAck
586+
activePacketBuffer = SFE_UBLOX_PACKET_PACKETACK;
587+
packetAck.cls = packetBuf.cls;
588+
packetAck.id = packetBuf.id;
589+
packetAck.len = packetBuf.len;
590+
packetAck.counter = packetBuf.counter;
591+
packetAck.payload[0] = packetBuf.payload[0];
592+
packetAck.payload[1] = packetBuf.payload[1];
593+
}
485594
}
486595
}
487596

488-
ubxFrameCounter++;
489-
490-
//Depending on this frame's class, pass different structs and payload arrays
491-
if (ubxFrameClass == CLASS_ACK)
597+
//Divert incoming into the correct buffer
598+
if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETACK)
492599
processUBX(incoming, &packetAck, requestedClass, requestedID);
493-
else if (ubxFrameClass == CLASS_NOT_AN_ACK)
600+
else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)
494601
processUBX(incoming, incomingUBX, requestedClass, requestedID);
602+
else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF)
603+
processUBX(incoming, &packetBuf, requestedClass, requestedID);
604+
605+
//Finally, increment the frame counter
606+
ubxFrameCounter++;
607+
495608
}
496609
else if (currentSentence == NMEA)
497610
{
@@ -589,54 +702,18 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t
589702
if (incomingUBX->counter == 0)
590703
{
591704
incomingUBX->cls = incoming;
592-
// if (_printDebug == true)
593-
// {
594-
// _debugSerial->print(F("processUBX: Class : 0x"));
595-
// _debugSerial->print(incomingUBX->cls, HEX);
596-
// _debugSerial->print(F(" CSUMA: 0x"));
597-
// _debugSerial->print(rollingChecksumA, HEX);
598-
// _debugSerial->print(F(" CSUMB: 0x"));
599-
// _debugSerial->println(rollingChecksumB, HEX);
600-
// }
601705
}
602706
else if (incomingUBX->counter == 1)
603707
{
604708
incomingUBX->id = incoming;
605-
// if (_printDebug == true)
606-
// {
607-
// _debugSerial->print(F("processUBX: ID : 0x"));
608-
// _debugSerial->print(incomingUBX->id, HEX);
609-
// _debugSerial->print(F(" CSUMA: 0x"));
610-
// _debugSerial->print(rollingChecksumA, HEX);
611-
// _debugSerial->print(F(" CSUMB: 0x"));
612-
// _debugSerial->println(rollingChecksumB, HEX);
613-
// }
614709
}
615710
else if (incomingUBX->counter == 2) //Len LSB
616711
{
617712
incomingUBX->len = incoming;
618-
// if (_printDebug == true)
619-
// {
620-
// _debugSerial->print(F("processUBX: LEN_LSB: 0x"));
621-
// _debugSerial->print(incomingUBX->len, HEX);
622-
// _debugSerial->print(F(" CSUMA: 0x"));
623-
// _debugSerial->print(rollingChecksumA, HEX);
624-
// _debugSerial->print(F(" CSUMB: 0x"));
625-
// _debugSerial->println(rollingChecksumB, HEX);
626-
// }
627713
}
628714
else if (incomingUBX->counter == 3) //Len MSB
629715
{
630716
incomingUBX->len |= incoming << 8;
631-
// if (_printDebug == true)
632-
// {
633-
// _debugSerial->print(F("processUBX: LEN_MSB: 0x"));
634-
// _debugSerial->print(incoming, HEX);
635-
// _debugSerial->print(F(" CSUMA: 0x"));
636-
// _debugSerial->print(rollingChecksumA, HEX);
637-
// _debugSerial->print(F(" CSUMB: 0x"));
638-
// _debugSerial->println(rollingChecksumB, HEX);
639-
// }
640717
}
641718
else if (incomingUBX->counter == incomingUBX->len + 4) //ChecksumA
642719
{
@@ -674,10 +751,10 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t
674751
incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_NOTACKNOWLEDGED; // If we have a match, set the classAndIDmatch flag to NOTACKNOWLEDGED
675752
if (_printDebug == true)
676753
{
677-
_debugSerial->print(F("processUBX: NACK received: Requested Class: "));
678-
_debugSerial->print(incomingUBX->payload[0]);
679-
_debugSerial->print(F(" Requested ID: "));
680-
_debugSerial->println(incomingUBX->payload[1]);
754+
_debugSerial->print(F("processUBX: NACK received: Requested Class: 0x"));
755+
_debugSerial->print(incomingUBX->payload[0], HEX);
756+
_debugSerial->print(F(" Requested ID: 0x"));
757+
_debugSerial->println(incomingUBX->payload[1], HEX);
681758
}
682759
}
683760

@@ -762,11 +839,19 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t
762839
{
763840
//Check to see if we have room for this byte
764841
if (((incomingUBX->counter - 4) - startingSpot) < MAX_PAYLOAD_SIZE) //If counter = 208, starting spot = 200, we're good to record.
765-
incomingUBX->payload[incomingUBX->counter - 4 - startingSpot] = incoming; //Store this byte into payload array
842+
{
843+
// Check if this is payload data which should be ignored
844+
if (ignoreThisPayload == false)
845+
{
846+
incomingUBX->payload[incomingUBX->counter - 4 - startingSpot] = incoming; //Store this byte into payload array
847+
}
848+
}
766849
}
767850
}
768851

852+
//Increment the counter
769853
incomingUBX->counter++;
854+
770855
if (incomingUBX->counter == MAX_PAYLOAD_SIZE)
771856
{
772857
//Something has gone very wrong
@@ -1105,7 +1190,10 @@ void SFE_UBLOX_GPS::printPacket(ubxPacket *packet)
11051190
else if (packet->cls == UBX_CLASS_MON) //0x0A
11061191
_debugSerial->print("MON");
11071192
else
1193+
{
1194+
_debugSerial->print(F("0x"));
11081195
_debugSerial->print(packet->cls, HEX);
1196+
}
11091197

11101198
_debugSerial->print(F(" ID:"));
11111199
if (packet->cls == UBX_CLASS_NAV && packet->id == UBX_NAV_PVT)
@@ -1115,7 +1203,10 @@ void SFE_UBLOX_GPS::printPacket(ubxPacket *packet)
11151203
else if (packet->cls == UBX_CLASS_CFG && packet->id == UBX_CFG_CFG)
11161204
_debugSerial->print("SAVE");
11171205
else
1206+
{
1207+
_debugSerial->print(F("0x"));
11181208
_debugSerial->print(packet->id, HEX);
1209+
}
11191210

11201211
_debugSerial->print(F(" Len: 0x"));
11211212
_debugSerial->print(packet->len, HEX);
@@ -1160,7 +1251,7 @@ void SFE_UBLOX_GPS::printPacket(ubxPacket *packet)
11601251
//Returns SFE_UBLOX_STATUS_DATA_RECEIVED if we got an ACK and a valid packetCfg (module is responding with register content)
11611252
//Returns SFE_UBLOX_STATUS_DATA_SENT if we got an ACK and no packetCfg (no valid packetCfg needed, module absorbs new register data)
11621253
//Returns SFE_UBLOX_STATUS_FAIL if something very bad happens (e.g. a double checksum failure)
1163-
//Returns SFE_UBLOX_STATUS_COMMAND_UNKNOWN if the packet was not-acknowledged (NACK)
1254+
//Returns SFE_UBLOX_STATUS_COMMAND_NACK if the packet was not-acknowledged (NACK)
11641255
//Returns SFE_UBLOX_STATUS_CRC_FAIL if we had a checksum failure
11651256
//Returns SFE_UBLOX_STATUS_TIMEOUT if we timed out
11661257
//Returns SFE_UBLOX_STATUS_DATA_OVERWRITTEN if we got an ACK and a valid packetCfg but that the packetCfg has been
@@ -1169,8 +1260,10 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(ubxPacket *outgoingUBX, uin
11691260
{
11701261
outgoingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent
11711262
packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
1263+
packetBuf.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
11721264
outgoingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; // This will go VALID (or NOT_VALID) when we receive a packet that matches the requested class and ID
11731265
packetAck.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
1266+
packetBuf.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
11741267

11751268
unsigned long startTime = millis();
11761269
while (millis() - startTime < maxTime)
@@ -1261,7 +1354,7 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(ubxPacket *outgoingUBX, uin
12611354
_debugSerial->print(millis() - startTime);
12621355
_debugSerial->println(F(" msec"));
12631356
}
1264-
return (SFE_UBLOX_STATUS_COMMAND_UNKNOWN); //We received a NACK!
1357+
return (SFE_UBLOX_STATUS_COMMAND_NACK); //We received a NACK!
12651358
}
12661359

12671360
// If the outgoingUBX->classAndIDmatch is VALID but the packetAck.classAndIDmatch is NOT_VALID
@@ -1352,8 +1445,10 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForNoACKResponse(ubxPacket *outgoingUBX, u
13521445
{
13531446
outgoingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent
13541447
packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
1448+
packetBuf.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
13551449
outgoingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; // This will go VALID (or NOT_VALID) when we receive a packet that matches the requested class and ID
13561450
packetAck.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
1451+
packetBuf.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
13571452

13581453
unsigned long startTime = millis();
13591454
while (millis() - startTime < maxTime)

‎src/SparkFun_Ublox_Arduino_Library.h‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -93,7 +93,7 @@ typedef enum
9393
SFE_UBLOX_STATUS_FAIL,
9494
SFE_UBLOX_STATUS_CRC_FAIL,
9595
SFE_UBLOX_STATUS_TIMEOUT,
96-
SFE_UBLOX_STATUS_COMMAND_UNKNOWN,
96+
SFE_UBLOX_STATUS_COMMAND_NACK, // Indicates that the command was unrecognised, invalid or that the module is too busy to respond
9797
SFE_UBLOX_STATUS_OUT_OF_RANGE,
9898
SFE_UBLOX_STATUS_INVALID_ARG,
9999
SFE_UBLOX_STATUS_INVALID_OPERATION,
@@ -742,13 +742,16 @@ class SFE_UBLOX_GPS
742742
//These are pointed at from within the ubxPacket
743743
uint8_t payloadAck[2]; // Holds the requested ACK/NACK
744744
uint8_t payloadCfg[MAX_PAYLOAD_SIZE]; // Holds the requested data packet
745-
uint8_t payloadBuf[2]; // Temporary buffer used to screen incoming packets (same size as Ack)
745+
uint8_t payloadBuf[2]; // Temporary buffer used to screen incoming packets or dump unrequested packets
746746

747747
//Init the packet structures and init them with pointers to the payloadAck, payloadCfg and payloadBuf arrays
748748
ubxPacket packetAck = {0, 0, 0, 0, 0, payloadAck, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
749749
ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
750750
ubxPacket packetBuf = {0, 0, 0, 0, 0, payloadBuf, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
751751

752+
//Flag if this packet is unrequested (and so should be ignored and not copied into packetCfg or packetAck)
753+
boolean ignoreThisPayload = false;
754+
752755
//Identify which buffer is in use
753756
//Data is stored in packetBuf until the requested class and ID can be validated
754757
//If a match is seen, data is diverted into packetAck or packetCfg

0 commit comments

Comments
 (0)