From e988f05aad92e9aa0c5a6468915eb184ce4cbed2 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 9 Sep 2022 00:25:06 +0100 Subject: [PATCH] Add helper functions for NAV-HPPOSECEF --- ...mple21_GetHighPrecisionECEFUsingDouble.ino | 121 ++++++++++++++++++ keywords.txt | 6 + src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 102 +++++++++++++++ src/SparkFun_u-blox_GNSS_Arduino_Library.h | 6 + 4 files changed, 235 insertions(+) create mode 100644 examples/ZED-F9P/Example21_GetHighPrecisionECEFUsingDouble/Example21_GetHighPrecisionECEFUsingDouble.ino diff --git a/examples/ZED-F9P/Example21_GetHighPrecisionECEFUsingDouble/Example21_GetHighPrecisionECEFUsingDouble.ino b/examples/ZED-F9P/Example21_GetHighPrecisionECEFUsingDouble/Example21_GetHighPrecisionECEFUsingDouble.ino new file mode 100644 index 0000000..538e33d --- /dev/null +++ b/examples/ZED-F9P/Example21_GetHighPrecisionECEFUsingDouble/Example21_GetHighPrecisionECEFUsingDouble.ino @@ -0,0 +1,121 @@ +/* + Get the high precision ECEF coordinates using double + By: Paul Clark + SparkFun Electronics + Date: September 8th, 2022 + License: MIT. See license file for more information but you can + basically do whatever you want with this code. + + This example shows how to read the high-precision ECEF + positional solution. Please see below for information about the units. + + ** This example will only work correctly on platforms which support 64-bit double ** + + Feel like supporting open source hardware? + Buy a board from SparkFun! + ZED-F9P RTK2: https://www.sparkfun.com/products/15136 + NEO-M8P RTK: https://www.sparkfun.com/products/15005 + + Hardware Connections: + Plug a Qwiic cable into the GNSS and (e.g.) a Redboard Artemis https://www.sparkfun.com/products/15444 + or an Artemis Thing Plus https://www.sparkfun.com/products/15574 + If you don't have a platform with a Qwiic connection use the SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425) + Open the serial monitor at 115200 baud to see the output +*/ + +#include // Needed for I2C to GNSS + +#define myWire Wire // This will work on the Redboard Artemis and the Artemis Thing Plus using Qwiic +//#define myWire Wire1 // Uncomment this line if you are using the extra SCL1/SDA1 pins (D17 and D16) on the Thing Plus + +#include //http://librarymanager/All#SparkFun_u-blox_GNSS +SFE_UBLOX_GNSS myGNSS; + +long lastTime = 0; //Simple local timer. Limits amount if I2C traffic to u-blox module. + +void setup() +{ + Serial.begin(115200); + while (!Serial); //Wait for user to open terminal + + myWire.begin(); + + //myGNSS.enableDebugging(Serial); // Uncomment this line to enable debug messages + + if (myGNSS.begin(myWire) == false) //Connect to the u-blox module using Wire port + { + Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing.")); + while (1) + ; + } + + // Check that this platform supports 64-bit (8 byte) double + if (sizeof(double) < 8) + { + Serial.println(F("Warning! Your platform does not support 64-bit double.")); + Serial.println(F("The ECEF coordinates will be inaccurate.")); + } + + myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) + //myGNSS.saveConfiguration(); //Save the current settings to flash and BBR +} + +void loop() +{ + //Query module only every second. + //The module only responds when a new position is available. + if (millis() - lastTime > 1000) + { + lastTime = millis(); //Update the timer + + // getHighResECEFX: returns the X coordinate from HPPOSECEF as an int32_t in cm + // getHighResECEFXHp: returns the high resolution component of the X coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm) + // getHighResECEFY: returns the Y coordinate from HPPOSECEF as an int32_t in cm + // getHighResECEFYHp: returns the high resolution component of the Y coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm) + // getHighResECEFZ: returns the Z coordinate from HPPOSECEF as an int32_t in cm + // getHighResECEFZHp: returns the high resolution component of the Z coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm) + // getPositionAccuracy: returns the position accuracy estimate from HPPOSLLH as an uint32_t in mm (note: not 0.1mm) + + // First, let's collect the position data + int32_t ECEFX = myGNSS.getHighResECEFX(); + int8_t ECEFXHp = myGNSS.getHighResECEFXHp(); + int32_t ECEFY = myGNSS.getHighResECEFY(); + int8_t ECEFYHp = myGNSS.getHighResECEFYHp(); + int32_t ECEFZ = myGNSS.getHighResECEFZ(); + int8_t ECEFZHp = myGNSS.getHighResECEFZHp(); + uint32_t accuracy = myGNSS.getPositionAccuracy(); + + // Defines storage for the ECEF coordinates as double + double d_ECEFX; + double d_ECEFY; + double d_ECEFZ; + + // Assemble the high precision coordinates + d_ECEFX = ((double)ECEFX) / 100.0; // Convert from cm to m + d_ECEFX += ((double)ECEFXHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 ) + d_ECEFY = ((double)ECEFY) / 100.0; // Convert from cm to m + d_ECEFY += ((double)ECEFYHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 ) + d_ECEFZ = ((double)ECEFZ) / 100.0; // Convert from cm to m + d_ECEFZ += ((double)ECEFZHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 ) + + // Print the coordinates with 4 decimal places (0.1mm) + Serial.print("X (m): "); + Serial.print(d_ECEFX, 4); + Serial.print(", Y (m): "); + Serial.print(d_ECEFY, 4); + Serial.print(", Z (m): "); + Serial.print(d_ECEFZ, 4); + + // Now define float storage for the accuracy + float f_accuracy; + + // Convert the horizontal accuracy (mm) to a float + f_accuracy = accuracy; + // Now convert to m + f_accuracy = f_accuracy / 1000.0; // Convert from mm to m + + // Finally, do the printing + Serial.print(", Accuracy (m): "); + Serial.println(f_accuracy, 3); // Print the accuracy with 3 decimal places + } +} diff --git a/keywords.txt b/keywords.txt index 206b751..3c1f60d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -585,6 +585,12 @@ getMagAcc KEYWORD2 getHeadVehValid KEYWORD2 getPositionAccuracy KEYWORD2 +getHighResECEFX KEYWORD2 +getHighResECEFY KEYWORD2 +getHighResECEFZ KEYWORD2 +getHighResECEFXHp KEYWORD2 +getHighResECEFYHp KEYWORD2 +getHighResECEFZHp KEYWORD2 getTimeOfWeekFromHPPOSLLH KEYWORD2 getHighResLongitude KEYWORD2 diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp index 6033a73..0377cf7 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp @@ -17527,6 +17527,108 @@ uint32_t SFE_UBLOX_GNSS::getPositionAccuracy(uint16_t maxWait) return (tempAccuracy); } +// Get the current 3D high precision X coordinate +// Returns a long representing the coordinate in cm +int32_t SFE_UBLOX_GNSS::getHighResECEFX(uint16_t maxWait) +{ + if (packetUBXNAVHPPOSECEF == NULL) + initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data + if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed + return 0; + + if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefX == false) + getNAVHPPOSECEF(maxWait); + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefX = false; // Since we are about to give this to user, mark this data as stale + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false; + + return (packetUBXNAVHPPOSECEF->data.ecefX); +} + +// Get the current 3D high precision Y coordinate +// Returns a long representing the coordinate in cm +int32_t SFE_UBLOX_GNSS::getHighResECEFY(uint16_t maxWait) +{ + if (packetUBXNAVHPPOSECEF == NULL) + initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data + if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed + return 0; + + if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefY == false) + getNAVHPPOSECEF(maxWait); + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefY = false; // Since we are about to give this to user, mark this data as stale + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false; + + return (packetUBXNAVHPPOSECEF->data.ecefY); +} + +// Get the current 3D high precision Z coordinate +// Returns a long representing the coordinate in cm +int32_t SFE_UBLOX_GNSS::getHighResECEFZ(uint16_t maxWait) +{ + if (packetUBXNAVHPPOSECEF == NULL) + initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data + if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed + return 0; + + if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZ == false) + getNAVHPPOSECEF(maxWait); + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZ = false; // Since we are about to give this to user, mark this data as stale + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false; + + return (packetUBXNAVHPPOSECEF->data.ecefZ); +} + +// Get the high precision component of the ECEF X coordinate +// Returns a signed byte representing the component as 0.1*mm +int8_t SFE_UBLOX_GNSS::getHighResECEFXHp(uint16_t maxWait) +{ + if (packetUBXNAVHPPOSECEF == NULL) + initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data + if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed + return 0; + + if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefXHp == false) + getNAVHPPOSECEF(maxWait); + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefXHp = false; // Since we are about to give this to user, mark this data as stale + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false; + + return (packetUBXNAVHPPOSECEF->data.ecefXHp); +} + +// Get the high precision component of the ECEF Y coordinate +// Returns a signed byte representing the component as 0.1*mm +int8_t SFE_UBLOX_GNSS::getHighResECEFYHp(uint16_t maxWait) +{ + if (packetUBXNAVHPPOSECEF == NULL) + initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data + if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed + return 0; + + if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefYHp == false) + getNAVHPPOSECEF(maxWait); + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefYHp = false; // Since we are about to give this to user, mark this data as stale + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false; + + return (packetUBXNAVHPPOSECEF->data.ecefYHp); +} + +// Get the high precision component of the ECEF Z coordinate +// Returns a signed byte representing the component as 0.1*mm +int8_t SFE_UBLOX_GNSS::getHighResECEFZHp(uint16_t maxWait) +{ + if (packetUBXNAVHPPOSECEF == NULL) + initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data + if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed + return 0; + + if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZHp == false) + getNAVHPPOSECEF(maxWait); + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZHp = false; // Since we are about to give this to user, mark this data as stale + packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false; + + return (packetUBXNAVHPPOSECEF->data.ecefZHp); +} + // ***** HPPOSLLH Helper Functions uint32_t SFE_UBLOX_GNSS::getTimeOfWeekFromHPPOSLLH(uint16_t maxWait) diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h index 763b86f..d405b08 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h @@ -1408,6 +1408,12 @@ public: // Helper functions for HPPOSECEF uint32_t getPositionAccuracy(uint16_t maxWait = defaultMaxWait); // Returns the 3D accuracy of the current high-precision fix, in mm. Supported on NEO-M8P, ZED-F9P, + int32_t getHighResECEFX(uint16_t maxWait = defaultMaxWait); // Returns the ECEF X coordinate (cm) + int32_t getHighResECEFY(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Y coordinate (cm) + int32_t getHighResECEFZ(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Z coordinate (cm) + int8_t getHighResECEFXHp(uint16_t maxWait = defaultMaxWait); // Returns the ECEF X coordinate High Precision Component (0.1 mm) + int8_t getHighResECEFYHp(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Y coordinate High Precision Component (0.1 mm) + int8_t getHighResECEFZHp(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Z coordinate High Precision Component (0.1 mm) // Helper functions for HPPOSLLH