From 97b0ce149e93ed2f71bcfcfca057b3d93833c7e9 Mon Sep 17 00:00:00 2001 From: Paul Date: Sat, 6 May 2023 08:47:27 +0100 Subject: [PATCH] Add callbacks for RTCM 1005 and 1006 Input --- ...xample17_NTRIPClient_With_GGA_Callback.ino | 107 +++--- keywords.txt | 2 + library.properties | 2 +- src/u-blox_GNSS.cpp | 322 ++++++++++-------- src/u-blox_GNSS.h | 24 +- 5 files changed, 257 insertions(+), 200 deletions(-) diff --git a/examples/ZED-F9P/Example17_NTRIPClient_With_GGA_Callback/Example17_NTRIPClient_With_GGA_Callback.ino b/examples/ZED-F9P/Example17_NTRIPClient_With_GGA_Callback/Example17_NTRIPClient_With_GGA_Callback.ino index 14b0ac9..a4d5c3f 100644 --- a/examples/ZED-F9P/Example17_NTRIPClient_With_GGA_Callback/Example17_NTRIPClient_With_GGA_Callback.ino +++ b/examples/ZED-F9P/Example17_NTRIPClient_With_GGA_Callback/Example17_NTRIPClient_With_GGA_Callback.ino @@ -144,6 +144,62 @@ void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= +// Callback: printRTCMdata1005 will be called when new RTCM 1005 data has been parsed from pushRawData +// See u-blox_structs.h for the full definition of RTCM_1005_data_t +// _____ You can use any name you like for the callback. Use the same name when you call setRTCM1005InputcallbackPtr +// / _____ This _must_ be RTCM_1005_data_t +// | / _____ You can use any name you like for the struct +// | | / +// | | | +void printRTCMdata1005(RTCM_1005_data_t *rtcmData1005) +{ + double x = rtcmData1005->AntennaReferencePointECEFX; + x /= 10000.0; // Convert to m + double y = rtcmData1005->AntennaReferencePointECEFY; + y /= 10000.0; // Convert to m + double z = rtcmData1005->AntennaReferencePointECEFZ; + z /= 10000.0; // Convert to m + + Serial.print(F("NTRIP Server RTCM 1005: ARP ECEF-X: ")); + Serial.print(x, 4); // 4 decimal places + Serial.print(F(" Y: ")); + Serial.print(y, 4); // 4 decimal places + Serial.print(F(" Z: ")); + Serial.println(z, 4); // 4 decimal places +} + +//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= + +// Callback: printRTCMdata1006 will be called when new RTCM 1006 data has been parsed from pushRawData +// See u-blox_structs.h for the full definition of RTCM_1006_data_t +// _____ You can use any name you like for the callback. Use the same name when you call setRTCM1006InputcallbackPtr +// / _____ This _must_ be RTCM_1006_data_t +// | / _____ You can use any name you like for the struct +// | | / +// | | | +void printRTCMdata1006(RTCM_1006_data_t *rtcmData1006) +{ + double x = rtcmData1006->AntennaReferencePointECEFX; + x /= 10000.0; // Convert to m + double y = rtcmData1006->AntennaReferencePointECEFY; + y /= 10000.0; // Convert to m + double z = rtcmData1006->AntennaReferencePointECEFZ; + z /= 10000.0; // Convert to m + double h = rtcmData1006->AntennaHeight; + h /= 10000.0; // Convert to m + + Serial.print(F("NTRIP Server RTCM 1006: ARP ECEF-X: ")); + Serial.print(x, 4); // 4 decimal places + Serial.print(F(" Y: ")); + Serial.print(y, 4); // 4 decimal places + Serial.print(F(" Z: ")); + Serial.print(z, 4); // 4 decimal places + Serial.print(F(" Height: ")); + Serial.println(h, 4); // 4 decimal places +} + +//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= + void setup() { delay(1000); @@ -176,6 +232,9 @@ void setup() myGNSS.setAutoPVTcallbackPtr(&printPVTdata); // Enable automatic NAV PVT messages with callback to printPVTdata so we can watch the carrier solution go to fixed + myGNSS.setRTCM1005InputcallbackPtr(&printRTCMdata1005); // Set up a callback to print the RTCM 1005 Antenna Reference Position from the correction data + myGNSS.setRTCM1006InputcallbackPtr(&printRTCMdata1006); // Set up a callback to print the RTCM 1006 Antenna Reference Position from the correction data + //myGNSS.saveConfiguration(VAL_CFG_SUBSEC_IOPORT | VAL_CFG_SUBSEC_MSGCONF); //Optional: Save the ioPort and message settings to NVM //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= @@ -230,54 +289,6 @@ void loop() //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= - // Check if the library has been able to extract the Antenna Reference Position from an RTCM 1005 message - - RTCM_1005_data_t rtcmData1005; - - if (myGNSS.getLatestRTCM1005Input(&rtcmData1005) == 2) // RTCM 1005 data received? 0 = no data, 1 = stale data, 2 = fresh data - { - double x = rtcmData1005.AntennaReferencePointECEFX; - x /= 10000.0; // Convert to m - double y = rtcmData1005.AntennaReferencePointECEFY; - y /= 10000.0; // Convert to m - double z = rtcmData1005.AntennaReferencePointECEFZ; - z /= 10000.0; // Convert to m - - Serial.print(F("NTRIP Server RTCM 1005: ARP ECEF-X: ")); - Serial.print(x, 4); // 4 decimal places - Serial.print(F(" Y: ")); - Serial.print(y, 4); // 4 decimal places - Serial.print(F(" Z: ")); - Serial.println(z, 4); // 4 decimal places - } - - // Check if the library has been able to extract the Antenna Reference Position from an RTCM 1006 message - - RTCM_1006_data_t rtcmData1006; - - if (myGNSS.getLatestRTCM1006Input(&rtcmData1006) == 2) // RTCM 1006 data received? 0 = no data, 1 = stale data, 2 = fresh data - { - double x = rtcmData1006.AntennaReferencePointECEFX; - x /= 10000.0; // Convert to m - double y = rtcmData1006.AntennaReferencePointECEFY; - y /= 10000.0; // Convert to m - double z = rtcmData1006.AntennaReferencePointECEFZ; - z /= 10000.0; // Convert to m - double h = rtcmData1006.AntennaHeight; - h /= 10000.0; // Convert to m - - Serial.print(F("NTRIP Server RTCM 1006: ARP ECEF-X: ")); - Serial.print(x, 4); // 4 decimal places - Serial.print(F(" Y: ")); - Serial.print(y, 4); // 4 decimal places - Serial.print(F(" Z: ")); - Serial.print(z, 4); // 4 decimal places - Serial.print(F(" Height: ")); - Serial.println(h, 4); // 4 decimal places - } - - //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= - switch (state) { case open_connection: diff --git a/keywords.txt b/keywords.txt index 51ffcf7..325b3a2 100644 --- a/keywords.txt +++ b/keywords.txt @@ -726,7 +726,9 @@ crc24q KEYWORD2 getLatestRTCM1005 KEYWORD2 setRTCM1005callbackPtr KEYWORD2 getLatestRTCM1005Input KEYWORD2 +setRTCM1005InputcallbackPtr KEYWORD2 getLatestRTCM1006Input KEYWORD2 +setRTCM1006InputcallbackPtr KEYWORD2 extractRTCM1005 KEYWORD2 extractRTCM1006 KEYWORD2 diff --git a/library.properties b/library.properties index 49e6691..12fdfd9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SparkFun u-blox GNSS v3 -version=3.0.11 +version=3.0.12 author=SparkFun Electronics maintainer=SparkFun Electronics sentence=Library for I2C, Serial and SPI Communication with u-blox GNSS modules

diff --git a/src/u-blox_GNSS.cpp b/src/u-blox_GNSS.cpp index 7f9d37d..37fab33 100644 --- a/src/u-blox_GNSS.cpp +++ b/src/u-blox_GNSS.cpp @@ -65,7 +65,7 @@ DevUBLOXGNSS::DevUBLOXGNSS(void) _logRTCM.all = 0; // Default to passing no RTCM messages to the file buffer #ifndef SFE_UBLOX_DISABLE_RTCM_LOGGING - rtcmInputStorage.flags.all = 0; // Clear the RTCM Input flags + rtcmInputStorage.init(); #endif } @@ -6048,6 +6048,22 @@ void DevUBLOXGNSS::checkCallbacks(void) storageRTCM1005->automaticFlags.flags.bits.callbackDataValid = 0; // Mark the data as stale } + if (rtcmInputStorage.rtcm1005CallbackPointer != nullptr) // If the pointer to the callback has been defined + if (rtcmInputStorage.flags.bits.dataValid1005 == 1) // If the copy of the data is valid + if (rtcmInputStorage.flags.bits.dataRead1005 == 0) // If the data has not been read + { + rtcmInputStorage.rtcm1005CallbackPointer(&rtcmInputStorage.rtcm1005); // Call the callback + rtcmInputStorage.flags.bits.dataRead1005 = 1; // Mark the data as read + } + + if (rtcmInputStorage.rtcm1006CallbackPointer != nullptr) // If the pointer to the callback has been defined + if (rtcmInputStorage.flags.bits.dataValid1006 == 1) // If the copy of the data is valid + if (rtcmInputStorage.flags.bits.dataRead1006 == 0) // If the data has not been read + { + rtcmInputStorage.rtcm1006CallbackPointer(&rtcmInputStorage.rtcm1006); // Call the callback + rtcmInputStorage.flags.bits.dataRead1006 = 1; // Mark the data as read + } + checkCallbacksReentrant = false; } @@ -7421,7 +7437,8 @@ void DevUBLOXGNSS::parseRTCM1005(uint8_t *dataBytes, size_t numDataBytes) static uint8_t rtcm1005store[RTCM_1005_MSG_LEN_BYTES + 6]; static uint8_t bytesStored; - enum parse1005states { + enum parse1005states + { waitingForD3, expecting00, expecting13, @@ -7435,77 +7452,77 @@ void DevUBLOXGNSS::parseRTCM1005(uint8_t *dataBytes, size_t numDataBytes) { switch (parse1005state) { - case waitingForD3: - if (*(dataBytes + i) == 0xD3) - { - rtcm1005store[0] = 0xD3; - parse1005state = expecting00; - } - break; - case expecting00: - if (*(dataBytes + i) == 0x00) - { - rtcm1005store[1] = 0x00; - parse1005state = expecting13; - } - else - { - parse1005state = waitingForD3; - } - break; - case expecting13: - if (*(dataBytes + i) == 0x13) - { - rtcm1005store[2] = 0x13; - parse1005state = expecting3E; - } - else - { - parse1005state = waitingForD3; - } - break; - case expecting3E: - if (*(dataBytes + i) == 0x3E) - { - rtcm1005store[3] = 0x3E; - parse1005state = expectingDn; - } - else - { - parse1005state = waitingForD3; - } - break; - case expectingDn: - if (((*(dataBytes + i)) & 0xF0) == 0xD0) - { - rtcm1005store[4] = *(dataBytes + i); - parse1005state = storingBytes; - bytesStored = 5; - } - else - { - parse1005state = waitingForD3; - } - break; - case storingBytes: - rtcm1005store[bytesStored++] = *(dataBytes + i); - if (bytesStored == RTCM_1005_MSG_LEN_BYTES + 6) // All data received? - { - parse1005state = waitingForD3; - uint32_t checksum = 0; - for (size_t j = 0; j < (RTCM_1005_MSG_LEN_BYTES + 3); j++) - crc24q(rtcm1005store[j], &checksum); - if (rtcm1005store[RTCM_1005_MSG_LEN_BYTES + 3] == ((checksum >> 16) & 0xFF)) // Check the checksum - if (rtcm1005store[RTCM_1005_MSG_LEN_BYTES + 4] == ((checksum >> 8) & 0xFF)) - if (rtcm1005store[RTCM_1005_MSG_LEN_BYTES + 5] == (checksum & 0xFF)) - { - extractRTCM1005(&rtcmInputStorage.rtcm1005, &rtcm1005store[3]); - rtcmInputStorage.flags.bits.dataValid1005 = 1; - rtcmInputStorage.flags.bits.dataRead1005 = 0; - return; // Return now - to avoid processing the remainder of the data - } - } - break; + case waitingForD3: + if (*(dataBytes + i) == 0xD3) + { + rtcm1005store[0] = 0xD3; + parse1005state = expecting00; + } + break; + case expecting00: + if (*(dataBytes + i) == 0x00) + { + rtcm1005store[1] = 0x00; + parse1005state = expecting13; + } + else + { + parse1005state = waitingForD3; + } + break; + case expecting13: + if (*(dataBytes + i) == 0x13) + { + rtcm1005store[2] = 0x13; + parse1005state = expecting3E; + } + else + { + parse1005state = waitingForD3; + } + break; + case expecting3E: + if (*(dataBytes + i) == 0x3E) + { + rtcm1005store[3] = 0x3E; + parse1005state = expectingDn; + } + else + { + parse1005state = waitingForD3; + } + break; + case expectingDn: + if (((*(dataBytes + i)) & 0xF0) == 0xD0) + { + rtcm1005store[4] = *(dataBytes + i); + parse1005state = storingBytes; + bytesStored = 5; + } + else + { + parse1005state = waitingForD3; + } + break; + case storingBytes: + rtcm1005store[bytesStored++] = *(dataBytes + i); + if (bytesStored == RTCM_1005_MSG_LEN_BYTES + 6) // All data received? + { + parse1005state = waitingForD3; + uint32_t checksum = 0; + for (size_t j = 0; j < (RTCM_1005_MSG_LEN_BYTES + 3); j++) + crc24q(rtcm1005store[j], &checksum); + if (rtcm1005store[RTCM_1005_MSG_LEN_BYTES + 3] == ((checksum >> 16) & 0xFF)) // Check the checksum + if (rtcm1005store[RTCM_1005_MSG_LEN_BYTES + 4] == ((checksum >> 8) & 0xFF)) + if (rtcm1005store[RTCM_1005_MSG_LEN_BYTES + 5] == (checksum & 0xFF)) + { + extractRTCM1005(&rtcmInputStorage.rtcm1005, &rtcm1005store[3]); + rtcmInputStorage.flags.bits.dataValid1005 = 1; + rtcmInputStorage.flags.bits.dataRead1005 = 0; + return; // Return now - to avoid processing the remainder of the data + } + } + break; } } } @@ -7518,7 +7535,8 @@ void DevUBLOXGNSS::parseRTCM1006(uint8_t *dataBytes, size_t numDataBytes) static uint8_t rtcm1006store[RTCM_1006_MSG_LEN_BYTES + 6]; static uint8_t bytesStored; - enum parse1006states { + enum parse1006states + { waitingForD3, expecting00, expecting15, @@ -7532,78 +7550,78 @@ void DevUBLOXGNSS::parseRTCM1006(uint8_t *dataBytes, size_t numDataBytes) { switch (parse1006state) { - case waitingForD3: - if (*(dataBytes + i) == 0xD3) - { - rtcm1006store[0] = 0xD3; - parse1006state = expecting00; - } - break; - case expecting00: - if (*(dataBytes + i) == 0x00) - { - rtcm1006store[1] = 0x00; - parse1006state = expecting15; - } - else - { - parse1006state = waitingForD3; - } - break; - case expecting15: - if (*(dataBytes + i) == 0x15) - { - rtcm1006store[2] = 0x15; - parse1006state = expecting3E; - } - else - { - parse1006state = waitingForD3; - } - break; - case expecting3E: - if (*(dataBytes + i) == 0x3E) - { - rtcm1006store[3] = 0x3E; - parse1006state = expectingEn; - } - else - { - parse1006state = waitingForD3; - } - break; - case expectingEn: - if (((*(dataBytes + i)) & 0xF0) == 0xE0) - { - rtcm1006store[4] = *(dataBytes + i); - parse1006state = storingBytes; - bytesStored = 5; - } - else - { - parse1006state = waitingForD3; - } - break; - case storingBytes: - rtcm1006store[bytesStored++] = *(dataBytes + i); - if (bytesStored == RTCM_1006_MSG_LEN_BYTES + 6) // All data received? - { - parse1006state = waitingForD3; - uint32_t checksum = 0; - for (size_t j = 0; j < (RTCM_1006_MSG_LEN_BYTES + 3); j++) - crc24q(rtcm1006store[j], &checksum); - - if (rtcm1006store[RTCM_1006_MSG_LEN_BYTES + 3] == ((checksum >> 16) & 0xFF)) // Check the checksum - if (rtcm1006store[RTCM_1006_MSG_LEN_BYTES + 4] == ((checksum >> 8) & 0xFF)) - if (rtcm1006store[RTCM_1006_MSG_LEN_BYTES + 5] == (checksum & 0xFF)) - { - extractRTCM1006(&rtcmInputStorage.rtcm1006, &rtcm1006store[3]); - rtcmInputStorage.flags.bits.dataValid1006 = 1; - rtcmInputStorage.flags.bits.dataRead1006 = 0; - return; // Return now - to avoid processing the remainder of the data - } - } - break; + case waitingForD3: + if (*(dataBytes + i) == 0xD3) + { + rtcm1006store[0] = 0xD3; + parse1006state = expecting00; + } + break; + case expecting00: + if (*(dataBytes + i) == 0x00) + { + rtcm1006store[1] = 0x00; + parse1006state = expecting15; + } + else + { + parse1006state = waitingForD3; + } + break; + case expecting15: + if (*(dataBytes + i) == 0x15) + { + rtcm1006store[2] = 0x15; + parse1006state = expecting3E; + } + else + { + parse1006state = waitingForD3; + } + break; + case expecting3E: + if (*(dataBytes + i) == 0x3E) + { + rtcm1006store[3] = 0x3E; + parse1006state = expectingEn; + } + else + { + parse1006state = waitingForD3; + } + break; + case expectingEn: + if (((*(dataBytes + i)) & 0xF0) == 0xE0) + { + rtcm1006store[4] = *(dataBytes + i); + parse1006state = storingBytes; + bytesStored = 5; + } + else + { + parse1006state = waitingForD3; + } + break; + case storingBytes: + rtcm1006store[bytesStored++] = *(dataBytes + i); + if (bytesStored == RTCM_1006_MSG_LEN_BYTES + 6) // All data received? + { + parse1006state = waitingForD3; + uint32_t checksum = 0; + for (size_t j = 0; j < (RTCM_1006_MSG_LEN_BYTES + 3); j++) + crc24q(rtcm1006store[j], &checksum); + + if (rtcm1006store[RTCM_1006_MSG_LEN_BYTES + 3] == ((checksum >> 16) & 0xFF)) // Check the checksum + if (rtcm1006store[RTCM_1006_MSG_LEN_BYTES + 4] == ((checksum >> 8) & 0xFF)) + if (rtcm1006store[RTCM_1006_MSG_LEN_BYTES + 5] == (checksum & 0xFF)) + { + extractRTCM1006(&rtcmInputStorage.rtcm1006, &rtcm1006store[3]); + rtcmInputStorage.flags.bits.dataValid1006 = 1; + rtcmInputStorage.flags.bits.dataRead1006 = 0; + return; // Return now - to avoid processing the remainder of the data + } + } + break; } } } @@ -16094,6 +16112,18 @@ uint8_t DevUBLOXGNSS::getLatestRTCM1006Input(RTCM_1006_data_t *data) return result; } +// Configure a callback for RTCM 1005 Input - from pushRawData +void DevUBLOXGNSS::setRTCM1005InputcallbackPtr(void (*rtcm1005CallbackPointer)(RTCM_1005_data_t *)) +{ + rtcmInputStorage.rtcm1005CallbackPointer = rtcm1005CallbackPointer; +} + +// Configure a callback for RTCM 1006 Input - from pushRawData +void DevUBLOXGNSS::setRTCM1006InputcallbackPtr(void (*rtcm1006CallbackPointer)(RTCM_1006_data_t *)) +{ + rtcmInputStorage.rtcm1006CallbackPointer = rtcm1006CallbackPointer; +} + #endif // ***** CFG RATE Helper Functions diff --git a/src/u-blox_GNSS.h b/src/u-blox_GNSS.h index 106dce4..094ad7d 100644 --- a/src/u-blox_GNSS.h +++ b/src/u-blox_GNSS.h @@ -271,6 +271,7 @@ class DevUBLOXGNSS protected: void parseRTCM1005(uint8_t *dataBytes, size_t numDataBytes); void parseRTCM1006(uint8_t *dataBytes, size_t numDataBytes); + public: #endif @@ -1267,8 +1268,10 @@ class DevUBLOXGNSS uint8_t getLatestRTCM1005(RTCM_1005_data_t *data); // Return the most recent RTCM 1005: 0 = no data, 1 = stale data, 2 = fresh data bool setRTCM1005callbackPtr(void (*callbackPointerPtr)(RTCM_1005_data_t *)); // Configure a callback for the RTCM 1005 Message - uint8_t getLatestRTCM1005Input(RTCM_1005_data_t *data); // Return the most recent RTCM 1005 Input, extracted from pushRawData: 0 = no data, 1 = stale data, 2 = fresh data - uint8_t getLatestRTCM1006Input(RTCM_1006_data_t *data); // Return the most recent RTCM 1006 Input, extracted from pushRawData: 0 = no data, 1 = stale data, 2 = fresh data + uint8_t getLatestRTCM1005Input(RTCM_1005_data_t *data); // Return the most recent RTCM 1005 Input, extracted from pushRawData: 0 = no data, 1 = stale data, 2 = fresh data + void setRTCM1005InputcallbackPtr(void (*rtcm1005CallbackPointer)(RTCM_1005_data_t *)); // Configure a callback for RTCM 1005 Input - from pushRawData + uint8_t getLatestRTCM1006Input(RTCM_1006_data_t *data); // Return the most recent RTCM 1006 Input, extracted from pushRawData: 0 = no data, 1 = stale data, 2 = fresh data + void setRTCM1006InputcallbackPtr(void (*rtcm1006CallbackPointer)(RTCM_1006_data_t *)); // Configure a callback for RTCM 1006 Input - from pushRawData void extractRTCM1005(RTCM_1005_data_t *destination, uint8_t *source); // Extract RTCM 1005 from source into destination void extractRTCM1006(RTCM_1006_data_t *destination, uint8_t *source); // Extract RTCM 1006 from source into destination @@ -1367,10 +1370,13 @@ class DevUBLOXGNSS RTCM_1005_t *storageRTCM1005 = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary #ifndef SFE_UBLOX_DISABLE_RTCM_LOGGING - struct { - union { + struct + { + union + { uint8_t all; - struct { + struct + { uint8_t dataValid1005 : 1; uint8_t dataRead1005 : 1; uint8_t dataValid1006 : 1; @@ -1379,6 +1385,14 @@ class DevUBLOXGNSS } flags; RTCM_1005_data_t rtcm1005; // Latest RTCM 1005 parsed from pushRawData RTCM_1006_data_t rtcm1006; // Latest RTCM 1006 parsed from pushRawData + void (*rtcm1005CallbackPointer)(RTCM_1005_data_t *); + void (*rtcm1006CallbackPointer)(RTCM_1006_data_t *); + void init(void) // Initializer / constructor + { + flags.all = 0; // Clear the RTCM Input flags + rtcm1005CallbackPointer = nullptr; // Clear the callback pointers + rtcm1006CallbackPointer = nullptr; + } } rtcmInputStorage; // Latest RTCM parsed from pushRawData #endif