Skip to content
This repository was archived by the owner on Jan 28, 2021. It is now read-only.

Commit 8cdf2d8

Browse files
PaulZCPaulZC
committed
Adding packetAuto. Work in progress.
1 parent 33e2c64 commit 8cdf2d8

File tree

5 files changed

+244
-43
lines changed

5 files changed

+244
-43
lines changed

‎examples/Dead Reckoning/Example5_getHNRData/Example5_getHNRData.ino‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -63,28 +63,28 @@ void loop()
6363
if (myGPS.getHNRAtt(125) == true) // Request HNR Att data using a 125ms timeout
6464
{
6565
Serial.print(F("Roll: "));
66-
Serial.print(myGPS.hnrAtt.roll);
66+
Serial.print(myGPS.packetUBXHNRATT->data.roll);
6767
Serial.print(F(" Pitch: "));
68-
Serial.print(myGPS.hnrAtt.pitch);
68+
Serial.print(myGPS.packetUBXHNRATT->data.pitch);
6969
Serial.print(F(" Heading: "));
70-
Serial.println(myGPS.hnrAtt.heading);
70+
Serial.println(myGPS.packetUBXHNRATT->data.heading);
7171
}
7272
if (myGPS.getHNRDyn(125) == true) // Request HNR Dyn data using a 125ms timeout
7373
{
7474
Serial.print(F("xAccel: "));
75-
Serial.print(myGPS.hnrVehDyn.xAccel);
75+
Serial.print(myGPS.packetUBXHNRINS->data.xAccel);
7676
Serial.print(F(" yAccel: "));
77-
Serial.print(myGPS.hnrVehDyn.yAccel);
77+
Serial.print(myGPS.packetUBXHNRINS->data.yAccel);
7878
Serial.print(F(" zAccel: "));
79-
Serial.println(myGPS.hnrVehDyn.zAccel);
79+
Serial.println(myGPS.packetUBXHNRINS->data.zAccel);
8080
}
8181
if (myGPS.getHNRPVT(125) == true) // Request HNR PVT data using a 125ms timeout
8282
{
8383
Serial.print(F("ns: "));
84-
Serial.print(myGPS.hnrPVT.nano);
84+
Serial.print(myGPS.packetUBXHNRPVT->data.nano);
8585
Serial.print(F(" Lat: "));
86-
Serial.print(myGPS.hnrPVT.lat);
86+
Serial.print(myGPS.packetUBXHNRPVT->data.lat);
8787
Serial.print(F(" Lon: "));
88-
Serial.println(myGPS.hnrPVT.lon);
88+
Serial.println(myGPS.packetUBXHNRPVT->data.lon);
8989
}
9090
}

‎examples/Dead Reckoning/Example6_getAutoHNRData/Example6_getAutoHNRData.ino‎

Lines changed: 13 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ void setup()
4343

4444
Wire.begin();
4545

46-
//myGPS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
46+
myGPS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
4747

4848
if (myGPS.begin() == false) //Connect to the Ublox module using Wire port
4949
{
@@ -69,28 +69,31 @@ void loop()
6969
if (usingAutoHNRAtt && (myGPS.getHNRAtt() == true)) // If setAutoHNRAtt was successful and new data is available
7070
{
7171
Serial.print(F("Roll: ")); // Print selected data
72-
Serial.print(myGPS.hnrAtt.roll);
72+
Serial.print(myGPS.packetUBXHNRATT->data.roll);
7373
Serial.print(F(" Pitch: "));
74-
Serial.print(myGPS.hnrAtt.pitch);
74+
Serial.print(myGPS.packetUBXHNRATT->data.pitch);
7575
Serial.print(F(" Heading: "));
76-
Serial.println(myGPS.hnrAtt.heading);
76+
Serial.println(myGPS.packetUBXHNRATT->data.heading);
77+
myGPS.flushHNRATT(); // Mark data as stale
7778
}
7879
if (usingAutoHNRDyn && (myGPS.getHNRDyn() == true)) // If setAutoHNRDyn was successful and new data is available
7980
{
8081
Serial.print(F("xAccel: ")); // Print selected data
81-
Serial.print(myGPS.hnrVehDyn.xAccel);
82+
Serial.print(myGPS.packetUBXHNRINS->data.xAccel);
8283
Serial.print(F(" yAccel: "));
83-
Serial.print(myGPS.hnrVehDyn.yAccel);
84+
Serial.print(myGPS.packetUBXHNRINS->data.yAccel);
8485
Serial.print(F(" zAccel: "));
85-
Serial.println(myGPS.hnrVehDyn.zAccel);
86+
Serial.println(myGPS.packetUBXHNRINS->data.zAccel);
87+
myGPS.flushHNRINS(); // Mark data as stale
8688
}
8789
if (usingAutoHNRPVT && (myGPS.getHNRPVT() == true)) // If setAutoHNRPVT was successful and new data is available
8890
{
8991
Serial.print(F("ns: ")); // Print selected data
90-
Serial.print(myGPS.hnrPVT.nano);
92+
Serial.print(myGPS.packetUBXHNRPVT->data.nano);
9193
Serial.print(F(" Lat: "));
92-
Serial.print(myGPS.hnrPVT.lat);
94+
Serial.print(myGPS.packetUBXHNRPVT->data.lat);
9395
Serial.print(F(" Lon: "));
94-
Serial.println(myGPS.hnrPVT.lon);
96+
Serial.println(myGPS.packetUBXHNRPVT->data.lon);
97+
myGPS.flushHNRPVT(); // Mark data as stale
9598
}
9699
}

‎src/SparkFun_Ublox_Arduino_Library.cpp‎

Lines changed: 202 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ void SFE_UBLOX_GPS::setPacketCfgPayloadSize(size_t payloadSize)
5959
{
6060
// Zero payloadSize? Dangerous! But we'll free the memory anyway...
6161
delete[] payloadCfg;
62+
payloadCfg = NULL; // Redundant?
6263
packetCfg.payload = payloadCfg;
6364
if ((_printDebug == true) || (_printLimitedDebug == true))
6465
_debugSerial->println(F("setPacketCfgPayloadSize: Zero payloadSize! This will end _very_ badly..."));
@@ -553,6 +554,139 @@ boolean SFE_UBLOX_GPS::checkAutomatic(uint8_t Class, uint8_t ID)
553554
return (result);
554555
}
555556

557+
//PRIVATE: Calculate how much RAM is needed to store the payload for a given automatic message
558+
uint16_t SFE_UBLOX_GPS::getMaxPayloadSize(uint8_t Class, uint8_t ID)
559+
{
560+
uint16_t maxSize = 0;
561+
switch (Class)
562+
{
563+
case UBX_CLASS_NAV:
564+
{
565+
switch (ID)
566+
{
567+
case UBX_NAV_POSECEF:
568+
maxSize = UBX_NAV_POSECEF_LEN;
569+
break;
570+
case UBX_NAV_POSLLH:
571+
maxSize = UBX_NAV_POSLLH_LEN;
572+
break;
573+
case UBX_NAV_STATUS:
574+
maxSize = UBX_NAV_STATUS_LEN;
575+
break;
576+
case UBX_NAV_DOP:
577+
maxSize = UBX_NAV_DOP_LEN;
578+
break;
579+
case UBX_NAV_ATT:
580+
maxSize = UBX_NAV_ATT_LEN;
581+
break;
582+
case UBX_NAV_PVT:
583+
maxSize = UBX_NAV_PVT_LEN;
584+
break;
585+
case UBX_NAV_ODO:
586+
maxSize = UBX_NAV_ODO_LEN;
587+
break;
588+
case UBX_NAV_VELECEF:
589+
maxSize = UBX_NAV_VELECEF_LEN;
590+
break;
591+
case UBX_NAV_VELNED:
592+
maxSize = UBX_NAV_VELNED_LEN;
593+
break;
594+
case UBX_NAV_HPPOSECEF:
595+
maxSize = UBX_NAV_HPPOSECEF_LEN;
596+
break;
597+
case UBX_NAV_HPPOSLLH:
598+
maxSize = UBX_NAV_HPPOSLLH_LEN;
599+
break;
600+
case UBX_NAV_TIMEUTC:
601+
maxSize = UBX_NAV_TIMEUTC_LEN;
602+
break;
603+
case UBX_NAV_CLOCK:
604+
maxSize = UBX_NAV_CLOCK_LEN;
605+
break;
606+
case UBX_NAV_SVIN:
607+
maxSize = UBX_NAV_SVIN_LEN;
608+
break;
609+
case UBX_NAV_RELPOSNED:
610+
maxSize = UBX_NAV_RELPOSNED_LEN_F9;
611+
break;
612+
}
613+
}
614+
break;
615+
case UBX_CLASS_RXM:
616+
{
617+
switch (ID)
618+
{
619+
case UBX_RXM_SFRBX:
620+
maxSize = UBX_RXM_SFRBX_MAX_LEN;
621+
break;
622+
case UBX_RXM_RAWX:
623+
maxSize = UBX_RXM_RAWX_MAX_LEN;
624+
break;
625+
}
626+
}
627+
break;
628+
case UBX_CLASS_CFG:
629+
{
630+
switch (ID)
631+
{
632+
case UBX_CFG_RATE:
633+
maxSize = UBX_CFG_RATE_LEN;
634+
break;
635+
}
636+
}
637+
break;
638+
case UBX_CLASS_TIM:
639+
{
640+
switch (ID)
641+
{
642+
case UBX_TIM_TM2:
643+
maxSize = UBX_TIM_TM2_LEN;
644+
break;
645+
}
646+
}
647+
break;
648+
case UBX_CLASS_ESF:
649+
{
650+
switch (ID)
651+
{
652+
case UBX_ESF_ALG:
653+
maxSize = UBX_ESF_ALG_LEN;
654+
break;
655+
case UBX_ESF_INS:
656+
maxSize = UBX_ESF_INS_LEN;
657+
break;
658+
case UBX_ESF_MEAS:
659+
maxSize = UBX_ESF_MEAS_MAX_LEN;
660+
break;
661+
case UBX_ESF_RAW:
662+
maxSize = UBX_ESF_RAW_MAX_LEN;
663+
break;
664+
case UBX_ESF_STATUS:
665+
maxSize = UBX_ESF_STATUS_MAX_LEN;
666+
break;
667+
}
668+
}
669+
break;
670+
case UBX_CLASS_HNR:
671+
{
672+
switch (ID)
673+
{
674+
case UBX_HNR_PVT:
675+
maxSize = UBX_HNR_PVT_LEN;
676+
break;
677+
case UBX_HNR_ATT:
678+
maxSize = UBX_HNR_ATT_LEN;
679+
break;
680+
case UBX_HNR_INS:
681+
maxSize = UBX_HNR_INS_LEN;
682+
break;
683+
}
684+
}
685+
break;
686+
}
687+
return (maxSize);
688+
}
689+
556690
//Processes NMEA and UBX binary sentences one byte at a time
557691
//Take a given byte and file it into the proper array
558692
void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID)
@@ -626,21 +760,56 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re
626760
incomingUBX->counter = packetBuf.counter; //Copy over the .counter too
627761
}
628762
//This is not an ACK and we do not have a complete class and ID match
629-
//So let's check for an "automatic" message which has its own storage defined
763+
//So let's check if this is an "automatic" message which has its own storage defined
630764
else if (checkAutomatic(packetBuf.cls, packetBuf.id))
631765
{
632-
//This is not the message we were expecting but it has its own storage and so
633-
//we start diverting data into incomingUBX (usually packetCfg) and process it anyway
634-
activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG;
635-
incomingUBX->cls = packetBuf.cls; //Copy the class and ID into incomingUBX (usually packetCfg)
636-
incomingUBX->id = packetBuf.id;
637-
incomingUBX->counter = packetBuf.counter; //Copy over the .counter too
638-
if (_printDebug == true)
766+
//This is not the message we were expecting but it has its own storage and so we should process it anyway.
767+
//We'll try to use packetAuto to buffer the message (so it can't overwrite anything in packetCfg).
768+
//We need to allocate memory for the packetAuto payload (payloadAuto) - and delete it once
769+
//reception is complete.
770+
uint16_t maxPayload = getMaxPayloadSize(packetBuf.cls, packetBuf.id); // Calculate how much RAM we need
771+
if (payloadAuto != NULL) // Check if memory is already allocated - this should be impossible!
639772
{
640-
_debugSerial->print(F("process: incoming \"automatic\" message: Class: 0x"));
641-
_debugSerial->print(packetBuf.cls, HEX);
642-
_debugSerial->print(F(" ID: 0x"));
643-
_debugSerial->println(packetBuf.id, HEX);
773+
if (_printDebug == true)
774+
{
775+
_debugSerial->println(F("process: memory is already allocated for payloadAuto! Deleting..."));
776+
}
777+
delete[] payloadAuto;
778+
payloadAuto = NULL; // Redundant?
779+
packetAuto.payload = payloadAuto;
780+
}
781+
payloadAuto = new uint8_t[maxPayload]; // Allocate RAM for payloadAuto
782+
packetAuto.payload = payloadAuto;
783+
if (payloadAuto == NULL) // Check if the alloc failed
784+
{
785+
if (_printDebug == true)
786+
{
787+
_debugSerial->print(F("process: memory allocation failed for \"automatic\" message: Class: 0x"));
788+
_debugSerial->print(packetBuf.cls, HEX);
789+
_debugSerial->print(F(" ID: 0x"));
790+
_debugSerial->println(packetBuf.id, HEX);
791+
_debugSerial->println(F("process: \"automatic\" message could overwrite data"));
792+
}
793+
// The RAM allocation failed so fall back to using incomingUBX (usually packetCfg) even though we risk overwriting data
794+
activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG;
795+
incomingUBX->cls = packetBuf.cls; //Copy the class and ID into incomingUBX (usually packetCfg)
796+
incomingUBX->id = packetBuf.id;
797+
incomingUBX->counter = packetBuf.counter; //Copy over the .counter too
798+
}
799+
else
800+
{
801+
//The RAM allocation was successful so we start diverting data into packetAuto and process it
802+
activePacketBuffer = SFE_UBLOX_PACKET_PACKETAUTO;
803+
packetAuto.cls = packetBuf.cls; //Copy the class and ID into packetAuto
804+
packetAuto.id = packetBuf.id;
805+
packetAuto.counter = packetBuf.counter; //Copy over the .counter too
806+
if (_printDebug == true)
807+
{
808+
_debugSerial->print(F("process: incoming \"automatic\" message: Class: 0x"));
809+
_debugSerial->print(packetBuf.cls, HEX);
810+
_debugSerial->print(F(" ID: 0x"));
811+
_debugSerial->println(packetBuf.id, HEX);
812+
}
644813
}
645814
}
646815
else
@@ -740,8 +909,10 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re
740909
processUBX(incoming, &packetAck, requestedClass, requestedID);
741910
else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)
742911
processUBX(incoming, incomingUBX, requestedClass, requestedID);
743-
else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF)
912+
else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF)
744913
processUBX(incoming, &packetBuf, requestedClass, requestedID);
914+
else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
915+
processUBX(incoming, &packetAuto, requestedClass, requestedID);
745916

746917
//Finally, increment the frame counter
747918
ubxFrameCounter++;
@@ -829,15 +1000,20 @@ void SFE_UBLOX_GPS::processRTCM(uint8_t incoming)
8291000

8301001
//Given a character, file it away into the uxb packet structure
8311002
//Set valid to VALID or NOT_VALID once sentence is completely received and passes or fails CRC
832-
//The payload portion of the packet can be 100s of bytes but the max array
833-
//size is packetCfgPayloadSize bytes. startingSpot can be set so we only record
834-
//a subset of bytes within a larger packet.
1003+
//The payload portion of the packet can be 100s of bytes but the max array size is packetCfgPayloadSize bytes.
1004+
//startingSpot can be set so we only record a subset of bytes within a larger packet.
8351005
void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID)
8361006
{
8371007
//If incomingUBX is a user-defined custom packet, then the payload size could be different to packetCfgPayloadSize.
8381008
//TO DO: update this to prevent an overrun when receiving an automatic message
8391009
// and the incomingUBX payload size is smaller than packetCfgPayloadSize.
840-
size_t max_payload_size = (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG) ? packetCfgPayloadSize : 2;
1010+
size_t max_payload_size;
1011+
if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)
1012+
max_payload_size = packetCfgPayloadSize;
1013+
else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
1014+
max_payload_size = getMaxPayloadSize(requestedClass, requestedID);
1015+
else
1016+
max_payload_size = 2;
8411017
bool overrun = false;
8421018

8431019
//Add all incoming bytes to the rolling checksum
@@ -995,6 +1171,15 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t
9951171
printPacket(incomingUBX);
9961172
}
9971173
}
1174+
1175+
// Now that the packet is complete and has been processed, we need to delete the memory
1176+
// allocated for packetAuto
1177+
if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
1178+
{
1179+
delete[] payloadAuto;
1180+
payloadAuto = NULL; // Redundant?
1181+
packetAuto.payload = payloadAuto;
1182+
}
9981183
}
9991184
else //Load this byte into the payload array
10001185
{

0 commit comments

Comments
 (0)