From fc33d0bc9330c6d26e25fce5463a10d33b40b743 Mon Sep 17 00:00:00 2001 From: Morten Nielsen Date: Thu, 29 Oct 2020 13:28:32 -0700 Subject: [PATCH] Adds support for auto-reporting DOP values --- src/SparkFun_Ublox_Arduino_Library.cpp | 243 ++++++++++++++++++++++++- src/SparkFun_Ublox_Arduino_Library.h | 46 ++++- 2 files changed, 277 insertions(+), 12 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 81994a1..061151d 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -553,8 +553,9 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re //This is not an ACK and we do not have a complete class and ID match //So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa else if ((packetBuf.cls == requestedClass) && - (((packetBuf.id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH)) || - ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT)))) + (((packetBuf.id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) || + ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || + ((packetBuf.id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH)))) { //This is not the message we were expecting but we start diverting data into incomingUBX (usually packetCfg) and process it anyway activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG; @@ -563,7 +564,7 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re incomingUBX->counter = packetBuf.counter; //Copy over the .counter too if (_printDebug == true) { - _debugSerial->print(F("process: auto PVT/HPPOSLLH collision: Requested ID: 0x")); + _debugSerial->print(F("process: auto PVT/HPPOSLLH/DOP collision: Requested ID: 0x")); _debugSerial->print(requestedID, HEX); _debugSerial->print(F(" Message ID: 0x")); _debugSerial->println(packetBuf.id, HEX); @@ -825,14 +826,15 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t //This is not an ACK and we do not have a complete class and ID match //So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa else if ((incomingUBX->cls == requestedClass) && - (((incomingUBX->id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH)) || - ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT)))) + (((incomingUBX->id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) || + ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || + ((incomingUBX->id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH)))) { // This isn't the message we are looking for... // Let's say so and leave incomingUBX->classAndIDmatch _unchanged_ if (_printDebug == true) { - _debugSerial->print(F("processUBX: auto PVT/HPPOSLLH collision: Requested ID: 0x")); + _debugSerial->print(F("processUBX: auto PVT/HPPOSLLH/DOP collision: Requested ID: 0x")); _debugSerial->print(requestedID, HEX); _debugSerial->print(F(" Message ID: 0x")); _debugSerial->println(incomingUBX->id, HEX); @@ -1080,6 +1082,24 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) } */ } + else if (msg->id == UBX_NAV_DOP && msg->len == 18) + { + geometricDOP = extractInt(4); + positionDOP = extractInt(6); + timeDOP = extractInt(8); + verticalDOP = extractInt(10); + horizontalDOP = extractInt(12); + northingDOP = extractInt(14); + eastingDOP = extractInt(16); + dopModuleQueried.all = true; + dopModuleQueried.geometricDOP = true; + dopModuleQueried.positionDOP = true; + dopModuleQueried.timeDOP = true; + dopModuleQueried.verticalDOP = true; + dopModuleQueried.horizontalDOP = true; + dopModuleQueried.northingDOP = true; + dopModuleQueried.eastingDOP = true; + } break; } } @@ -2403,6 +2423,49 @@ boolean SFE_UBLOX_GPS::setAutoHPPOSLLH(boolean enable, boolean implicitUpdate, u return ok; } + +//In case no config access to the GPS is possible and DOP is send cyclically already +//set config to suitable parameters +boolean SFE_UBLOX_GPS::assumeAutoDOP(boolean enabled, boolean implicitUpdate) +{ + boolean changes = autoDOP != enabled || autoDOPImplicitUpdate != implicitUpdate; + if (changes) + { + autoDOP = enabled; + autoDOPImplicitUpdate = implicitUpdate; + } + return changes; +} + +//Enable or disable automatic navigation message generation by the GPS. This changes the way getDOP +//works. +boolean SFE_UBLOX_GPS::setAutoDOP(boolean enable, uint16_t maxWait) +{ + return setAutoDOP(enable, true, maxWait); +} + +//Enable or disable automatic navigation message generation by the GPS. This changes the way getDOP +//works. +boolean SFE_UBLOX_GPS::setAutoDOP(boolean enable, boolean implicitUpdate, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_MSG; + packetCfg.len = 3; + packetCfg.startingSpot = 0; + payloadCfg[0] = UBX_CLASS_NAV; + payloadCfg[1] = UBX_NAV_DOP; + payloadCfg[2] = enable ? 1 : 0; // rate relative to navigation freq. + + boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK + if (ok) + { + autoDOP = enable; + autoDOPImplicitUpdate = implicitUpdate; + } + dopModuleQueried.all = false; + return ok; +} + //Configure a given message type for a given port (UART1, I2C, SPI, etc) boolean SFE_UBLOX_GPS::configureMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint8_t sendRate, uint16_t maxWait) { @@ -3054,6 +3117,15 @@ boolean SFE_UBLOX_GPS::getPVT(uint16_t maxWait) return (true); } + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_DOP)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getPVT: data was OVERWRITTEN by DOP (but that's OK)")); + } + return (true); + } + if (_printDebug == true) { _debugSerial->print(F("getPVT retVal: ")); @@ -3229,6 +3301,14 @@ boolean SFE_UBLOX_GPS::getHPPOSLLH(uint16_t maxWait) } return (true); } + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_DOP)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getHPPOSLLH: data was OVERWRITTEN by DOP (but that's OK)")); + } + return (true); + } if (_printDebug == true) { @@ -3239,6 +3319,141 @@ boolean SFE_UBLOX_GPS::getHPPOSLLH(uint16_t maxWait) } } +uint16_t SFE_UBLOX_GPS::getGeometricDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.geometricDOP == false) + getDOP(maxWait); + dopModuleQueried.geometricDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (geometricDOP); +} + +uint16_t SFE_UBLOX_GPS::getPositionDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.positionDOP == false) + getDOP(maxWait); + dopModuleQueried.positionDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (positionDOP); +} + +uint16_t SFE_UBLOX_GPS::getTimeDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.timeDOP == false) + getDOP(maxWait); + dopModuleQueried.timeDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (timeDOP); +} + +uint16_t SFE_UBLOX_GPS::getVerticalDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.verticalDOP == false) + getDOP(maxWait); + dopModuleQueried.verticalDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (verticalDOP); +} + +uint16_t SFE_UBLOX_GPS::getHorizontalDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.horizontalDOP == false) + getDOP(maxWait); + dopModuleQueried.horizontalDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (horizontalDOP); +} + +uint16_t SFE_UBLOX_GPS::getNorthingDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.northingDOP == false) + getDOP(maxWait); + dopModuleQueried.northingDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (northingDOP); +} + +uint16_t SFE_UBLOX_GPS::getEastingDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.eastingDOP == false) + getDOP(maxWait); + dopModuleQueried.eastingDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (eastingDOP); +} + +boolean SFE_UBLOX_GPS::getDOP(uint16_t maxWait) +{ + if (autoDOP && autoDOPImplicitUpdate) + { + //The GPS is automatically reporting, we just check whether we got unread data + if (_printDebug == true) + { + _debugSerial->println(F("getDOP: Autoreporting")); + } + checkUbloxInternal(&packetCfg, UBX_CLASS_NAV, UBX_NAV_DOP); + return dopModuleQueried.all; + } + else if (autoDOP && !autoDOPImplicitUpdate) + { + //Someone else has to call checkUblox for us... + if (_printDebug == true) + { + _debugSerial->println(F("getDOP: Exit immediately")); + } + return (false); + } + else + { + if (_printDebug == true) + { + _debugSerial->println(F("getDOP: Polling")); + } + + //The GPS is not automatically reporting navigation position so we have to poll explicitly + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_DOP; + packetCfg.len = 0; + + //The data is parsed as part of processing the response + sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait); + + if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED) + return (true); + + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_PVT)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getHPPOSLLH: data was OVERWRITTEN by PVT (but that's OK)")); + } + return (true); + } + + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_HPPOSLLH)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getPVT: data was OVERWRITTEN by HPPOSLLH (but that's OK)")); + } + return (true); + } + + if (_printDebug == true) + { + _debugSerial->print(F("getDOP retVal: ")); + _debugSerial->println(statusString(retVal)); + } + return (false); + } +} //Get the current 3D high precision positional accuracy - a fun thing to watch //Returns a long representing the 3D accuracy in millimeters uint32_t SFE_UBLOX_GPS::getPositionAccuracy(uint16_t maxWait) @@ -3496,6 +3711,20 @@ void SFE_UBLOX_GPS::flushHPPOSLLH() //moduleQueried.gpsiTOW = false; // this can arrive via HPPOS too. } +//Mark all the DOP data as read/stale. This is handy to get data alignment after CRC failure +void SFE_UBLOX_GPS::flushDOP() +{ + //Mark all DOPs as stale (read before) + dopModuleQueried.all = false; + dopModuleQueried.geometricDOP = false; + dopModuleQueried.positionDOP = false; + dopModuleQueried.timeDOP = false; + dopModuleQueried.verticalDOP = false; + dopModuleQueried.horizontalDOP = false; + dopModuleQueried.northingDOP = false; + dopModuleQueried.eastingDOP = false; +} + //Relative Positioning Information in NED frame //Returns true if commands was successful boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait) @@ -3800,4 +4029,4 @@ bool SFE_UBLOX_GPS::setStaticPosition(int32_t ecefXOrLat, int8_t ecefXOrLatHP, i bool SFE_UBLOX_GPS::setStaticPosition(int32_t ecefXOrLat, int32_t ecefYOrLon, int32_t ecefZOrAlt, bool latlong, uint16_t maxWait) { return (setStaticPosition(ecefXOrLat, 0, ecefYOrLon, 0, ecefZOrAlt, 0, latlong, maxWait)); -} \ No newline at end of file +} diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 8feadfc..d3d48f0 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -56,16 +56,15 @@ // Boards like the RedBoard Turbo use SerialUSB (not Serial). // But other boards like the SAMD51 Thing Plus use Serial (not SerialUSB). // The next nine lines let the code compile cleanly on as many SAMD boards as possible. -#if defined(ARDUINO_ARCH_SAMD) // Is this a SAMD board? -#if defined(USB_VID) // Is the USB Vendor ID defined? -#if (USB_VID == 0x1B4F) // Is this a SparkFun board? +#if defined(ARDUINO_ARCH_SAMD) // Is this a SAMD board? +#if defined(USB_VID) // Is the USB Vendor ID defined? +#if (USB_VID == 0x1B4F) // Is this a SparkFun board? #if !defined(ARDUINO_SAMD51_THING_PLUS) & !defined(ARDUINO_SAMD51_MICROMOD) // If it is not a SAMD51 Thing Plus or SAMD51 MicroMod -#define Serial SerialUSB // Define Serial as SerialUSB +#define Serial SerialUSB // Define Serial as SerialUSB #endif #endif #endif #endif - //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //Define a digital pin to aid checksum failure capture and analysis @@ -492,6 +491,7 @@ class SFE_UBLOX_GPS // The same is true for getHPPOSLLH. #define getPVTmaxWait 1100 // Default maxWait for getPVT and all functions which call it #define getHPPOSLLHmaxWait 1100 // Default maxWait for getHPPOSLLH and all functions which call it +#define getDOPmaxWait 1100 // Default maxWait for getDOP and all functions which all it boolean assumeAutoPVT(boolean enabled, boolean implicitUpdate = true); //In case no config access to the GPS is possible and PVT is send cyclically already boolean setAutoPVT(boolean enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic PVT reports at the navigation frequency @@ -501,8 +501,13 @@ class SFE_UBLOX_GPS boolean setAutoHPPOSLLH(boolean enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic HPPOSLLH reports at the navigation frequency boolean setAutoHPPOSLLH(boolean enabled, boolean implicitUpdate, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic HPPOSLLH reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update boolean getHPPOSLLH(uint16_t maxWait = getHPPOSLLHmaxWait); //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Returns true if new HPPOSLLH is available. + boolean assumeAutoDOP(boolean enabled, boolean implicitUpdate = true); //In case no config access to the GPS is possible and DOP is send cyclically already + boolean setAutoDOP(boolean enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic DOP reports at the navigation frequency + boolean setAutoDOP(boolean enabled, boolean implicitUpdate, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic DOP reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update + boolean getDOP(uint16_t maxWait = getDOPmaxWait); //Query module for latest dilution of precision values and load global vars:. If autoDOP is disabled, performs an explicit poll and waits, if enabled does not block. Returns true if new DOP is available. void flushPVT(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure void flushHPPOSLLH(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure + void flushDOP(); //Mark all the DOP data as read/stale. This is handy to get data alignment after CRC failure int32_t getLatitude(uint16_t maxWait = getPVTmaxWait); //Returns the current latitude in degrees * 10^-7. Auto selects between HighPrecision and Regular depending on ability of module. int32_t getLongitude(uint16_t maxWait = getPVTmaxWait); //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module. @@ -538,6 +543,14 @@ class SFE_UBLOX_GPS uint32_t getHorizontalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); uint32_t getVerticalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); + uint16_t getGeometricDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getPositionDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getTimeDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getVerticalDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getHorizontalDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getNorthingDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getEastingDOP(uint16_t maxWait = getDOPmaxWait); + //Port configurations boolean setPortOutput(uint8_t portID, uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure a given port to output UBX, NMEA, RTCM3 or a combination thereof boolean setPortInput(uint8_t portID, uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure a given port to input UBX, NMEA, RTCM3 or a combination thereof @@ -715,6 +728,14 @@ class SFE_UBLOX_GPS uint16_t rtcmFrameCounter = 0; //Tracks the type of incoming byte inside RTCM frame +uint16_t geometricDOP; // Geometric dilution of precision * 10^-2 +uint16_t positionDOP; // Posoition dilution of precision * 10^-2 +uint16_t timeDOP; // Time dilution of precision * 10^-2 +uint16_t verticalDOP; // Vertical dilution of precision * 10^-2 +uint16_t horizontalDOP; // Horizontal dilution of precision * 10^-2 +uint16_t northingDOP; // Northing dilution of precision * 10^-2 +uint16_t eastingDOP; // Easting dilution of precision * 10^-2 + #define DEF_NUM_SENS 7 struct deadReckData { @@ -850,6 +871,9 @@ class SFE_UBLOX_GPS boolean autoPVTImplicitUpdate = true; // Whether autoPVT is triggered by accessing stale data (=true) or by a call to checkUblox (=false) boolean autoHPPOSLLH = false; //Whether autoHPPOSLLH is enabled or not boolean autoHPPOSLLHImplicitUpdate = true; // Whether autoHPPOSLLH is triggered by accessing stale data (=true) or by a call to checkUblox (=false) + boolean autoDOP = false; //Whether autoDOP is enabled or not + boolean autoDOPImplicitUpdate = true; // Whether autoDOP is triggered by accessing stale data (=true) or by a call to checkUblox (=false) + uint16_t ubxFrameCounter; //It counts all UBX frame. [Fixed header(2bytes), CLS(1byte), ID(1byte), length(2bytes), payload(x bytes), checksums(2bytes)] uint8_t rollingChecksumA; //Rolls forward as we receive incoming bytes. Checked against the last two A/B checksum bytes @@ -903,6 +927,18 @@ class SFE_UBLOX_GPS uint16_t highResLongitudeHp : 1; } highResModuleQueried; + struct + { + uint16_t all : 1; + uint16_t geometricDOP : 1; + uint16_t positionDOP : 1; + uint16_t timeDOP : 1; + uint16_t verticalDOP : 1; + uint16_t horizontalDOP : 1; + uint16_t northingDOP : 1; + uint16_t eastingDOP : 1; + } dopModuleQueried; + uint16_t rtcmLen = 0; };