From 8f6b2c7c158c55ea0ba63c0fcbcae6db6a722c7a Mon Sep 17 00:00:00 2001 From: Paul <5690545+PaulZC@users.noreply.github.com> Date: Fri, 17 Apr 2020 12:42:22 +0100 Subject: [PATCH] Corrected the high precision functions --- ...e8_GetHighPrecisionPositionAndAccuracy.ino | 116 +++++++++++++++--- ...xample9_GetHighPrecisionGeodeticPacket.ino | 38 +++--- keywords.txt | 4 + src/SparkFun_Ublox_Arduino_Library.cpp | 68 ++++++++-- src/SparkFun_Ublox_Arduino_Library.h | 31 +++-- 5 files changed, 200 insertions(+), 57 deletions(-) diff --git a/examples/ZED-F9P/Example8_GetHighPrecisionPositionAndAccuracy/Example8_GetHighPrecisionPositionAndAccuracy.ino b/examples/ZED-F9P/Example8_GetHighPrecisionPositionAndAccuracy/Example8_GetHighPrecisionPositionAndAccuracy.ino index 9da0c1a..735588f 100644 --- a/examples/ZED-F9P/Example8_GetHighPrecisionPositionAndAccuracy/Example8_GetHighPrecisionPositionAndAccuracy.ino +++ b/examples/ZED-F9P/Example8_GetHighPrecisionPositionAndAccuracy/Example8_GetHighPrecisionPositionAndAccuracy.ino @@ -1,14 +1,14 @@ /* - Get the high precision geodetic solution for latituude and longitude + Get the high precision geodetic solution for latitude and longitude By: Nathan Seidle - Modified by: Steven Rowland + Modified by: Steven Rowland and Paul Clark SparkFun Electronics - Date: January 3rd, 2019 + Date: April 17th, 2020 License: MIT. See license file for more information but you can basically do whatever you want with this code. This example shows how to inspect the accuracy of the high-precision - positional solution. + positional solution. Please see below for information about the units. Feel like supporting open source hardware? Buy a board from SparkFun! @@ -36,22 +36,22 @@ void setup() Wire.begin(); - if (myGPS.begin() == false) //Connect to the Ublox module using Wire port + //myGPS.enableDebugging(Serial); + + if (myGPS.begin(Wire) == false) //Connect to the Ublox module using Wire port { Serial.println(F("Ublox GPS not detected at default I2C address. Please check wiring. Freezing.")); while (1); } - //myGPS.enableDebugging(Serial); - myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) myGPS.setNavigationFrequency(20); //Set output to 20 times a second byte rate = myGPS.getNavigationFrequency(); //Get the update rate of this module - Serial.print("Current update rate:"); + Serial.print("Current update rate: "); Serial.println(rate); - - myGPS.saveConfiguration(); //Save the current settings to flash and BBR + + //myGPS.saveConfiguration(); //Save the current settings to flash and BBR } void loop() @@ -61,17 +61,95 @@ void loop() if (millis() - lastTime > 1000) { lastTime = millis(); //Update the timer - Serial.print("HP Lat: "); - int32_t latitude = myGPS.getHighResLatitude(); - Serial.print(latitude); - Serial.print(", HP Lon: "); - int32_t longitude = myGPS.getHighResLongitude(); - Serial.print(longitude); - Serial.print(", Accuracy: "); + // getHighResLatitude: returns the latitude from HPPOSLLH as an int32_t in degrees * 10^-7 + // getHighResLatitudeHp: returns the high resolution component of latitude from HPPOSLLH as an int8_t in degrees * 10^-9 + // getHighResLongitude: returns the longitude from HPPOSLLH as an int32_t in degrees * 10^-7 + // getHighResLongitudeHp: returns the high resolution component of longitude from HPPOSLLH as an int8_t in degrees * 10^-9 + // getElipsoid: returns the height above ellipsoid as an int32_t in mm + // getElipsoidHp: returns the high resolution component of the height above ellipsoid as an int8_t in mm * 10^-1 + // getMeanSeaLevel: returns the height above mean sea level as an int32_t in mm + // getMeanSeaLevelHp: returns the high resolution component of the height above mean sea level as an int8_t in mm * 10^-1 + // getHorizontalAccuracy: returns the horizontal accuracy estimate from HPPOSLLH as an uint32_t in mm * 10^-1 + // If you want to use the high precision latitude and longitude with the full 9 decimal places + // you will need to use a 64-bit double - which is not supported on all platforms + + // To allow this example to run on standard platforms, we cheat by converting lat and lon to integer and fractional degrees + + // The high resolution altitudes can be converted into standard 32-bit float + + // First, let's collect the position data + int32_t latitude = myGPS.getHighResLatitude(); + int8_t latitudeHp = myGPS.getHighResLatitudeHp(); + int32_t longitude = myGPS.getHighResLongitude(); + int8_t longitudeHp = myGPS.getHighResLongitudeHp(); + int32_t ellipsoid = myGPS.getElipsoid(); + int8_t ellipsoidHp = myGPS.getElipsoidHp(); + int32_t msl = myGPS.getMeanSeaLevel(); + int8_t mslHp = myGPS.getMeanSeaLevelHp(); uint32_t accuracy = myGPS.getHorizontalAccuracy(); - Serial.println(accuracy); - } + // Defines storage for the lat and lon units integer and fractional parts + int32_t lat_int; // Integer part of the latitude in degrees + int32_t lat_frac; // Fractional part of the latitude + int32_t lon_int; // Integer part of the longitude in degrees + int32_t lon_frac; // Fractional part of the longitude + + // Calculate the latitude and longitude integer and fractional parts + lat_int = latitude / 10000000; // Convert latitude from degrees * 10^-7 to Degrees + lat_frac = latitude - (lat_int * 10000000); // Calculate the fractional part of the latitude + lat_frac = (lat_frac * 100) + latitudeHp; // Now add the high resolution component + if (lat_frac < 0) // If the fractional part is negative, remove the minus sign + { + lat_frac = 0 - lat_frac; + } + lon_int = longitude / 10000000; // Convert latitude from degrees * 10^-7 to Degrees + lon_frac = longitude - (lon_int * 10000000); // Calculate the fractional part of the longitude + lon_frac = (lon_frac * 100) + longitudeHp; // Now add the high resolution component + if (lon_frac < 0) // If the fractional part is negative, remove the minus sign + { + lon_frac = 0 - lon_frac; + } + + // Print the lat and lon + Serial.print("Lat (deg): "); + Serial.print(lat_int); // Print the integer part of the latitude + Serial.print("."); + Serial.print(lat_frac); // Print the fractional part of the latitude + Serial.print(", Lon (deg): "); + Serial.print(lon_int); // Print the integer part of the latitude + Serial.print("."); + Serial.println(lon_frac); // Print the fractional part of the latitude + + // Now define float storage for the heights and accuracy + float f_ellipsoid; + float f_msl; + float f_accuracy; + + // Calculate the height above ellipsoid in mm * 10^-1 + f_ellipsoid = (ellipsoid * 10) + ellipsoidHp; + // Now convert to m + f_ellipsoid = f_ellipsoid / 10000.0; // Convert from mm * 10^-1 to m + + // Calculate the height above mean sea level in mm * 10^-1 + f_msl = (msl * 10) + mslHp; + // Now convert to m + f_msl = f_msl / 10000.0; // Convert from mm * 10^-1 to m + + // Convert the horizontal accuracy (mm * 10^-1) to a float + f_accuracy = accuracy; + // Now convert to m + f_accuracy = f_accuracy / 10000.0; // Convert from mm * 10^-1 to m + + // Finally, do the printing + Serial.print("Ellipsoid (m): "); + Serial.print(f_ellipsoid, 4); // Print the ellipsoid with 4 decimal places + + Serial.print(", Mean Sea Level(m): "); + Serial.print(f_msl, 4); // Print the mean sea level with 4 decimal places + + Serial.print(", Accuracy (m): "); + Serial.println(f_accuracy, 4); // Print the accuracy with 4 decimal places + } } diff --git a/examples/ZED-F9P/Example9_GetHighPrecisionGeodeticPacket/Example9_GetHighPrecisionGeodeticPacket.ino b/examples/ZED-F9P/Example9_GetHighPrecisionGeodeticPacket/Example9_GetHighPrecisionGeodeticPacket.ino index b2cc6f7..24169b7 100644 --- a/examples/ZED-F9P/Example9_GetHighPrecisionGeodeticPacket/Example9_GetHighPrecisionGeodeticPacket.ino +++ b/examples/ZED-F9P/Example9_GetHighPrecisionGeodeticPacket/Example9_GetHighPrecisionGeodeticPacket.ino @@ -1,9 +1,9 @@ /* Get the high precision geodetic solution By: Nathan Seidle - Modified by: Steven Rowland + Modified by: Steven Rowland and Paul Clark SparkFun Electronics - Date: January 3rd, 2019 + Date: April 17th, 2020 License: MIT. See license file for more information but you can basically do whatever you want with this code. @@ -50,8 +50,8 @@ void setup() byte rate = myGPS.getNavigationFrequency(); //Get the update rate of this module Serial.print("Current update rate:"); Serial.println(rate); - - myGPS.saveConfiguration(); //Save the current settings to flash and BBR + + //myGPS.saveConfiguration(); //Save the current settings to flash and BBR } void loop() @@ -64,36 +64,30 @@ void loop() Serial.print("HP Lat: "); int32_t latitude = myGPS.getHighResLatitude(); Serial.print(latitude); - Serial.print(", HP Lon: "); + Serial.print(", HP Lon: "); int32_t longitude = myGPS.getHighResLongitude(); Serial.print(longitude); - Serial.print(", 2D Accuracy(MM): "); + Serial.print(", Horizontal Accuracy(0.1mm): "); uint32_t accuracy = myGPS.getHorizontalAccuracy(); - Serial.println(accuracy); - Serial.print(", Vertical Accuracy(MM): "); + Serial.print(accuracy); + Serial.print(", Vertical Accuracy(0.1mm): "); uint32_t verticalAccuracy = myGPS.getVerticalAccuracy(); Serial.println(verticalAccuracy); - Serial.print(", Elipsoid(MM): "); - + + Serial.print("Elipsoid(mm): "); int32_t elipsoid = myGPS.getElipsoid(); - Serial.println(elipsoid); - Serial.print(", Mean Sea Level(MM): "); - + Serial.print(elipsoid); + + Serial.print(", Mean Sea Level(mm): "); int32_t meanSeaLevel = myGPS.getMeanSeaLevel(); - Serial.println(meanSeaLevel); - Serial.print(", Geoid Separation(MM): "); - - int32_t geoidSeparation = myGPS.getGeoidSeparation(); - Serial.println(geoidSeparation); - Serial.print(", Time of Week(Millis): "); - + Serial.print(meanSeaLevel); + + Serial.print(", Time of Week(millis): "); uint32_t timeOfWeek = myGPS.getTimeOfWeek(); Serial.println(timeOfWeek); - Serial.print(","); - } } diff --git a/keywords.txt b/keywords.txt index d6ce26b..3bdbcf1 100644 --- a/keywords.txt +++ b/keywords.txt @@ -118,9 +118,13 @@ getNanosecond KEYWORD2 getHPPOSLLH KEYWORD2 getTimeOfWeek KEYWORD2 getHighResLatitude KEYWORD2 +getHighResLatitudeHp KEYWORD2 getHighResLongitude KEYWORD2 +getHighResLongitudeHp KEYWORD2 getElipsoid KEYWORD2 +getElipsoidHp KEYWORD2 getMeanSeaLevel KEYWORD2 +getMeanSeaLevelHp KEYWORD2 getGeoidSeparation KEYWORD2 getHorizontalAccuracy KEYWORD2 getVerticalAccuracy KEYWORD2 diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 673ee2d..c395918 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -785,15 +785,22 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) highResLatitude = extractLong(12); elipsoid = extractLong(16); meanSeaLevel = extractLong(20); - geoidSeparation = extractLong(24); + highResLongitudeHp = extractSignedChar(24); + highResLatitudeHp = extractSignedChar(25); + elipsoidHp = extractSignedChar(26); + meanSeaLevelHp = extractSignedChar(27); horizontalAccuracy = extractLong(28); verticalAccuracy = extractLong(32); highResModuleQueried.all = true; highResModuleQueried.highResLatitude = true; + highResModuleQueried.highResLatitudeHp = true; highResModuleQueried.highResLongitude = true; + highResModuleQueried.highResLongitudeHp = true; highResModuleQueried.elipsoid = true; + highResModuleQueried.elipsoidHp = true; highResModuleQueried.meanSeaLevel = true; + highResModuleQueried.meanSeaLevelHp = true; highResModuleQueried.geoidSeparation = true; highResModuleQueried.horizontalAccuracy = true; highResModuleQueried.verticalAccuracy = true; @@ -816,15 +823,23 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) _debugSerial->print(F("MSL M: ")); _debugSerial->print(((float)(int32_t)extractLong(20)) / 1000.0f); _debugSerial->print(F(" ")); - _debugSerial->print(F("GEO: ")); - _debugSerial->print(((float)(int32_t)extractLong(24)) / 1000.0f); + _debugSerial->print(F("LON HP: ")); + _debugSerial->print(extractSignedChar(24)); + _debugSerial->print(F(" ")); + _debugSerial->print(F("LAT HP: ")); + _debugSerial->print(extractSignedChar(25)); + _debugSerial->print(F(" ")); + _debugSerial->print(F("ELI HP: ")); + _debugSerial->print(extractSignedChar(26)); + _debugSerial->print(F(" ")); + _debugSerial->print(F("MSL HP: ")); + _debugSerial->print(extractSignedChar(27)); _debugSerial->print(F(" ")); _debugSerial->print(F("HA 2D M: ")); - _debugSerial->print(((float)extractLong(28)) / 1000.0f); + _debugSerial->print(((float)(int32_t)extractLong(28)) / 10000.0f); _debugSerial->print(F(" ")); _debugSerial->print(F("VERT M: ")); - _debugSerial->print(((float)extractLong(32)) / 1000.0f); - _debugSerial->print(F(" ")); + _debugSerial->println(((float)(int32_t)extractLong(32)) / 10000.0f); } } break; @@ -2287,12 +2302,18 @@ uint16_t SFE_UBLOX_GPS::extractInt(uint8_t spotToStart) return (val); } -//Given a spot, extract byte the payload +//Given a spot, extract a byte from the payload uint8_t SFE_UBLOX_GPS::extractByte(uint8_t spotToStart) { return (payloadCfg[spotToStart]); } +//Given a spot, extract a signed 8-bit value from the payload +int8_t SFE_UBLOX_GPS::extractSignedChar(uint8_t spotToStart) +{ + return ((int8_t)payloadCfg[spotToStart]); +} + //Get the current year uint16_t SFE_UBLOX_GPS::getYear(uint16_t maxWait) { @@ -2431,6 +2452,14 @@ int32_t SFE_UBLOX_GPS::getHighResLatitude(uint16_t maxWait /* = 250*/) return (highResLatitude); } +int8_t SFE_UBLOX_GPS::getHighResLatitudeHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.highResLatitudeHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.highResLatitudeHp = false; //Since we are about to give this to user, mark this data as stale + return (highResLatitudeHp); +} + int32_t SFE_UBLOX_GPS::getHighResLongitude(uint16_t maxWait /* = 250*/) { if (highResModuleQueried.highResLongitude == false) @@ -2439,6 +2468,14 @@ int32_t SFE_UBLOX_GPS::getHighResLongitude(uint16_t maxWait /* = 250*/) return (highResLongitude); } +int8_t SFE_UBLOX_GPS::getHighResLongitudeHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.highResLongitudeHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.highResLongitudeHp = false; //Since we are about to give this to user, mark this data as stale + return (highResLongitudeHp); +} + int32_t SFE_UBLOX_GPS::getElipsoid(uint16_t maxWait /* = 250*/) { if (highResModuleQueried.elipsoid == false) @@ -2447,6 +2484,14 @@ int32_t SFE_UBLOX_GPS::getElipsoid(uint16_t maxWait /* = 250*/) return (elipsoid); } +int8_t SFE_UBLOX_GPS::getElipsoidHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.elipsoidHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.elipsoidHp = false; //Since we are about to give this to user, mark this data as stale + return (elipsoidHp); +} + int32_t SFE_UBLOX_GPS::getMeanSeaLevel(uint16_t maxWait /* = 250*/) { if (highResModuleQueried.meanSeaLevel == false) @@ -2455,6 +2500,15 @@ int32_t SFE_UBLOX_GPS::getMeanSeaLevel(uint16_t maxWait /* = 250*/) return (meanSeaLevel); } +int8_t SFE_UBLOX_GPS::getMeanSeaLevelHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.meanSeaLevelHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.meanSeaLevelHp = false; //Since we are about to give this to user, mark this data as stale + return (meanSeaLevelHp); +} + +// getGeoidSeparation is currently redundant. The geoid separation seems to only be provided in NMEA GGA and GNS messages. int32_t SFE_UBLOX_GPS::getGeoidSeparation(uint16_t maxWait /* = 250*/) { if (highResModuleQueried.geoidSeparation == false) diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 0939e03..32a7ed7 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -519,9 +519,13 @@ class SFE_UBLOX_GPS uint32_t getTimeOfWeek(uint16_t maxWait = getPVTmaxWait); int32_t getHighResLatitude(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getHighResLatitudeHp(uint16_t maxWait = getHPPOSLLHmaxWait); int32_t getHighResLongitude(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getHighResLongitudeHp(uint16_t maxWait = getHPPOSLLHmaxWait); int32_t getElipsoid(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getElipsoidHp(uint16_t maxWait = getHPPOSLLHmaxWait); int32_t getMeanSeaLevel(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getMeanSeaLevelHp(uint16_t maxWait = getHPPOSLLHmaxWait); int32_t getGeoidSeparation(uint16_t maxWait = getHPPOSLLHmaxWait); uint32_t getHorizontalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); uint32_t getVerticalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); @@ -662,14 +666,18 @@ class SFE_UBLOX_GPS uint8_t versionLow; //Loaded from getProtocolVersion(). uint8_t versionHigh; - uint32_t timeOfWeek; - int32_t highResLatitude; - int32_t highResLongitude; - int32_t elipsoid; - int32_t meanSeaLevel; - int32_t geoidSeparation; - uint32_t horizontalAccuracy; - uint32_t verticalAccuracy; + uint32_t timeOfWeek; // ms + int32_t highResLatitude; // Degrees * 10^-7 + int32_t highResLongitude; // Degrees * 10^-7 + int32_t elipsoid; // Height above ellipsoid in mm (Typo! Should be eLLipsoid! **Uncorrected for backward-compatibility.**) + int32_t meanSeaLevel; // Height above mean sea level in mm + int32_t geoidSeparation; // This seems to only be provided in NMEA GGA and GNS messages + uint32_t horizontalAccuracy; // mm * 10^-1 (i.e. 0.1mm) + uint32_t verticalAccuracy; // mm * 10^-1 (i.e. 0.1mm) + int8_t elipsoidHp; // High precision component of the height above ellipsoid in mm * 10^-1 (Deliberate typo! Should be eLLipsoidHp!) + int8_t meanSeaLevelHp; // High precision component of Height above mean sea level in mm * 10^-1 + int8_t highResLatitudeHp; // High precision component of latitude: Degrees * 10^-9 + int8_t highResLongitudeHp; // High precision component of longitude: Degrees * 10^-9 uint16_t rtcmFrameCounter = 0; //Tracks the type of incoming byte inside RTCM frame @@ -702,6 +710,7 @@ class SFE_UBLOX_GPS uint32_t extractLong(uint8_t spotToStart); //Combine four bytes from payload into long uint16_t extractInt(uint8_t spotToStart); //Combine two bytes from payload into int uint8_t extractByte(uint8_t spotToStart); //Get byte from payload + int8_t extractSignedChar(uint8_t spotToStart); //Get signed 8-bit value from payload void addToChecksum(uint8_t incoming); //Given an incoming byte, adjust rollingChecksumA/B //Variables @@ -774,9 +783,13 @@ class SFE_UBLOX_GPS uint16_t highResLongitude : 1; uint16_t elipsoid : 1; uint16_t meanSeaLevel : 1; - uint16_t geoidSeparation : 1; + uint16_t geoidSeparation : 1; // Redundant but kept for backward-compatibility uint16_t horizontalAccuracy : 1; uint16_t verticalAccuracy : 1; + uint16_t elipsoidHp : 1; + uint16_t meanSeaLevelHp : 1; + uint16_t highResLatitudeHp : 1; + uint16_t highResLongitudeHp : 1; } highResModuleQueried; uint16_t rtcmLen = 0;