From 459f89b7279899f9edbcf1c0ebd7a3baca372590 Mon Sep 17 00:00:00 2001 From: Paul <5690545+PaulZC@users.noreply.github.com> Date: Tue, 10 Dec 2019 20:20:49 +0000 Subject: [PATCH] Added the geofence functions Added addGeofence, clearGeofences and getGeofenceState to allow up to four geofences to be defined. The geofence status can also be routed to one of the PIO pins and used as an interrupt etc.. --- README.md | 2 +- .../Example18_Geofence/Example18_Geofence.ino | 168 ++++++++++++++++++ keywords.txt | 4 + src/SparkFun_Ublox_Arduino_Library.cpp | 151 ++++++++++++++++ src/SparkFun_Ublox_Arduino_Library.h | 29 +++ 5 files changed, 353 insertions(+), 1 deletion(-) create mode 100644 examples/Example18_Geofence/Example18_Geofence.ino diff --git a/README.md b/README.md index e809c2b..608e3dd 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ Thanks to: * [tve](https://github.com/tve) for building out serial additions and examples * [Redstoned](https://github.com/Redstoned) and [davidallenmann](https://github.com/davidallenmann) for adding PVT date and time * [wittend](https://forum.sparkfun.com/viewtopic.php?t=49874) for pointing out the RTCM print bug -* Big thanks to [PaulZC](https://github.com/PaulZC) for implementing the combined key ValSet method +* Big thanks to [PaulZC](https://github.com/PaulZC) for implementing the combined key ValSet method and geofence functions * [RollieRowland](https://github.com/RollieRowland) for adding HPPOSLLH (High Precision Geodetic Position) * [tedder](https://github.com/tedder) for moving iTOW to PVT instead of HPPOS and comment cleanup diff --git a/examples/Example18_Geofence/Example18_Geofence.ino b/examples/Example18_Geofence/Example18_Geofence.ino new file mode 100644 index 0000000..a65f239 --- /dev/null +++ b/examples/Example18_Geofence/Example18_Geofence.ino @@ -0,0 +1,168 @@ +/* + u-blox M8 geofence example + + Written by Paul Clark (PaulZC) + 10th December 2019 + + License: MIT. See license file for more information but you can + basically do whatever you want with this code. + + This example demonstrates how to use the addGeofence and getGeofenceState functions + + 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 + SAM-M8Q: https://www.sparkfun.com/products/15210 + ZOE-M8Q: https://www.sparkfun.com/products/15193 + + This example powers up the GPS and reads the fix. + Once a valid 3D fix has been found, the code reads the latitude and longitude. + The code then sets four geofences around that position with a radii of 5m, 10m, 15m and 20m with 95% confidence. + The code then monitors the geofence status. + The LED will be illuminated if you are inside the _combined_ geofence (i.e. within the 20m radius). + + This code has been tested on the ZOE-M8Q. +*/ + +#define LED LED_BUILTIN // Change this if your LED is on a different pin + +#include // Needed for I2C + +#include "SparkFun_Ublox_Arduino_Library.h" //http://librarymanager/All#SparkFun_Ublox_GPS +SFE_UBLOX_GPS myGPS; + +void setup() +{ + pinMode(LED, OUTPUT); + + // Set up the I2C pins + Wire.begin(); + + // Start the console serial port + Serial.begin(115200); + while (!Serial); // Wait for the user to open the serial monitor + delay(100); + Serial.println(); + Serial.println(); + Serial.println(F("u-blox M8 geofence example")); + Serial.println(); + Serial.println(); + + delay(1000); // Let the GPS power up + + if (myGPS.begin() == 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(); // Enable debug messages + myGPS.setI2COutput(COM_TYPE_UBX); // Limit I2C output to UBX (disable the NMEA noise) + + Serial.println(F("Waiting for a 3D fix...")); + + byte fixType = 0; + + while (fixType != 3) + { + fixType = myGPS.getFixType(); // Get the fix type + Serial.print(F("Fix: ")); // Print it + Serial.print(fixType); + if(fixType == 0) Serial.print(F(" = No fix")); + else if(fixType == 1) Serial.print(F(" = Dead reckoning")); + else if(fixType == 2) Serial.print(F(" = 2D")); + else if(fixType == 3) Serial.print(F(" = 3D")); + else if(fixType == 4) Serial.print(F(" = GNSS + Dead reckoning")); + Serial.println(); + delay(1000); + } + + Serial.println(F("3D fix found!")); + + long latitude = myGPS.getLatitude(); // Get the latitude in degrees * 10^-7 + Serial.print(F("Lat: ")); + Serial.print(latitude); + + long longitude = myGPS.getLongitude(); // Get the longitude in degrees * 10^-7 + Serial.print(F(" Long: ")); + Serial.println(longitude); + + uint32_t radius = 500; // Set the radius to 5m (radius is in m * 10^-2 i.e. cm) + + byte confidence = 2; // Set the confidence level: 0=none, 1=68%, 2=95%, 3=99.7%, 4=99.99% + + // Call clearGeofences() to clear all existing geofences. + Serial.print(F("Clearing any existing geofences. clearGeofences returned: ")); + Serial.println(myGPS.clearGeofences()); + + // It is possible to define up to four geofences. + // Call addGeofence up to four times to define them. + Serial.println(F("Setting the geofences:")); + + Serial.print(F("addGeofence for geofence 1 returned: ")); + Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence)); + + radius = 1000; // 10m + Serial.print(F("addGeofence for geofence 2 returned: ")); + Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence)); + + radius = 1500; // 15m + Serial.print(F("addGeofence for geofence 3 returned: ")); + Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence)); + + radius = 2000; // 20m + Serial.print(F("addGeofence for geofence 4 returned: ")); + Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence)); +} + +void loop() +{ + geofenceState currentGeofenceState; // Create storage for the geofence state + + boolean result = myGPS.getGeofenceState(currentGeofenceState); + + Serial.print(F("getGeofenceState returned: ")); // Print the combined state + Serial.print(result); // Get the geofence state + + if (!result) // If getGeofenceState did not return true + { + Serial.println(F(".")); // Tidy up + return; // and go round the loop again + } + + Serial.print(F(". status is: ")); // Print the status + Serial.print(currentGeofenceState.status); + + Serial.print(F(". numFences is: ")); // Print the numFences + Serial.print(currentGeofenceState.numFences); + + Serial.print(F(". combState is: ")); // Print the combined state + Serial.print(currentGeofenceState.combState); + + if (currentGeofenceState.combState == 0) + { + Serial.print(F(" = Unknown")); + digitalWrite(LED, LOW); + } + if (currentGeofenceState.combState == 1) + { + Serial.print(F(" = Inside")); + digitalWrite(LED, HIGH); + } + else if (currentGeofenceState.combState == 2) + { + Serial.print(F(" = Outside")); + digitalWrite(LED, LOW); + } + + Serial.print(F(". The individual states are: ")); // Print the state of each geofence + for(int i = 0; i < currentGeofenceState.numFences; i++) + { + if (i > 0) Serial.print(F(",")); + Serial.print(currentGeofenceState.states[i]); + } + Serial.println(); + + delay(1000); +} diff --git a/keywords.txt b/keywords.txt index e1c7046..f0d1ed7 100644 --- a/keywords.txt +++ b/keywords.txt @@ -121,6 +121,10 @@ getGeoidSeparation KEYWORD2 getHorizontalAccuracy KEYWORD2 getVerticalAccuracy KEYWORD2 +addGeofence KEYWORD2 +clearGeofences KEYWORD2 +getGeofenceState KEYWORD2 + ####################################### # Constants (LITERAL1) ####################################### diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index ac0ca06..5fc9e9d 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -41,6 +41,7 @@ SFE_UBLOX_GPS::SFE_UBLOX_GPS(void) { // Constructor + currentGeofenceParams.numFences = 0; // Zero the number of geofences currently in use } //Initialize the Serial port @@ -1638,6 +1639,156 @@ boolean SFE_UBLOX_GPS::setAutoPVT(boolean enable, boolean implicitUpdate, uint16 return ok; } +//Add a new geofence using UBX-CFG-GEOFENCE +boolean SFE_UBLOX_GPS::addGeofence(int32_t latitude, int32_t longitude, uint32_t radius, byte confidence, byte pinPolarity, byte pin, uint16_t maxWait) +{ + if (currentGeofenceParams.numFences >= 4) return(false); // Quit if we already have four geofences defined + + // Store the new geofence parameters + currentGeofenceParams.lats[currentGeofenceParams.numFences] = latitude; + currentGeofenceParams.longs[currentGeofenceParams.numFences] = longitude; + currentGeofenceParams.rads[currentGeofenceParams.numFences] = radius; + currentGeofenceParams.numFences = currentGeofenceParams.numFences + 1; // Increment the number of fences + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_GEOFENCE; + packetCfg.len = (currentGeofenceParams.numFences * 12) + 8; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0; // Message version = 0x00 + payloadCfg[1] = currentGeofenceParams.numFences; // numFences + payloadCfg[2] = confidence; // confLvl = Confidence level 0-4 (none, 68%, 95%, 99.7%, 99.99%) + payloadCfg[3] = 0; // reserved1 + if (pin > 0) + { + payloadCfg[4] = 1; // enable PIO combined fence state + } + else + { + payloadCfg[4] = 0; // disable PIO combined fence state + } + payloadCfg[5] = pinPolarity; // PIO pin polarity (0 = low means inside, 1 = low means outside (or unknown)) + payloadCfg[6] = pin; // PIO pin + payloadCfg[7] = 0; //reserved2 + payloadCfg[8] = currentGeofenceParams.lats[0] & 0xFF; + payloadCfg[9] = currentGeofenceParams.lats[0] >> 8; + payloadCfg[10] = currentGeofenceParams.lats[0] >> 16; + payloadCfg[11] = currentGeofenceParams.lats[0] >> 24; + payloadCfg[12] = currentGeofenceParams.longs[0] & 0xFF; + payloadCfg[13] = currentGeofenceParams.longs[0] >> 8; + payloadCfg[14] = currentGeofenceParams.longs[0] >> 16; + payloadCfg[15] = currentGeofenceParams.longs[0] >> 24; + payloadCfg[16] = currentGeofenceParams.rads[0] & 0xFF; + payloadCfg[17] = currentGeofenceParams.rads[0] >> 8; + payloadCfg[18] = currentGeofenceParams.rads[0] >> 16; + payloadCfg[19] = currentGeofenceParams.rads[0] >> 24; + if (currentGeofenceParams.numFences >= 2) { + payloadCfg[20] = currentGeofenceParams.lats[1] & 0xFF; + payloadCfg[21] = currentGeofenceParams.lats[1] >> 8; + payloadCfg[22] = currentGeofenceParams.lats[1] >> 16; + payloadCfg[23] = currentGeofenceParams.lats[1] >> 24; + payloadCfg[24] = currentGeofenceParams.longs[1] & 0xFF; + payloadCfg[25] = currentGeofenceParams.longs[1] >> 8; + payloadCfg[26] = currentGeofenceParams.longs[1] >> 16; + payloadCfg[27] = currentGeofenceParams.longs[1] >> 24; + payloadCfg[28] = currentGeofenceParams.rads[1] & 0xFF; + payloadCfg[29] = currentGeofenceParams.rads[1] >> 8; + payloadCfg[30] = currentGeofenceParams.rads[1] >> 16; + payloadCfg[31] = currentGeofenceParams.rads[1] >> 24; + } + if (currentGeofenceParams.numFences >= 3) { + payloadCfg[32] = currentGeofenceParams.lats[2] & 0xFF; + payloadCfg[33] = currentGeofenceParams.lats[2] >> 8; + payloadCfg[34] = currentGeofenceParams.lats[2] >> 16; + payloadCfg[35] = currentGeofenceParams.lats[2] >> 24; + payloadCfg[36] = currentGeofenceParams.longs[2] & 0xFF; + payloadCfg[37] = currentGeofenceParams.longs[2] >> 8; + payloadCfg[38] = currentGeofenceParams.longs[2] >> 16; + payloadCfg[39] = currentGeofenceParams.longs[2] >> 24; + payloadCfg[40] = currentGeofenceParams.rads[2] & 0xFF; + payloadCfg[41] = currentGeofenceParams.rads[2] >> 8; + payloadCfg[42] = currentGeofenceParams.rads[2] >> 16; + payloadCfg[43] = currentGeofenceParams.rads[2] >> 24; + } + if (currentGeofenceParams.numFences >= 4) { + payloadCfg[44] = currentGeofenceParams.lats[3] & 0xFF; + payloadCfg[45] = currentGeofenceParams.lats[3] >> 8; + payloadCfg[46] = currentGeofenceParams.lats[3] >> 16; + payloadCfg[47] = currentGeofenceParams.lats[3] >> 24; + payloadCfg[48] = currentGeofenceParams.longs[3] & 0xFF; + payloadCfg[49] = currentGeofenceParams.longs[3] >> 8; + payloadCfg[50] = currentGeofenceParams.longs[3] >> 16; + payloadCfg[51] = currentGeofenceParams.longs[3] >> 24; + payloadCfg[52] = currentGeofenceParams.rads[3] & 0xFF; + payloadCfg[53] = currentGeofenceParams.rads[3] >> 8; + payloadCfg[54] = currentGeofenceParams.rads[3] >> 16; + payloadCfg[55] = currentGeofenceParams.rads[3] >> 24; + } + return (sendCommand(packetCfg, maxWait)); //Wait for ack +} + +//Clear all geofences using UBX-CFG-GEOFENCE +boolean SFE_UBLOX_GPS::clearGeofences(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_GEOFENCE; + packetCfg.len = 8; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0; // Message version = 0x00 + payloadCfg[1] = 0; // numFences + payloadCfg[2] = 0; // confLvl + payloadCfg[3] = 0; // reserved1 + payloadCfg[4] = 0; // disable PIO combined fence state + payloadCfg[5] = 0; // PIO pin polarity (0 = low means inside, 1 = low means outside (or unknown)) + payloadCfg[6] = 0; // PIO pin + payloadCfg[7] = 0; //reserved2 + + currentGeofenceParams.numFences = 0; // Zero the number of geofences currently in use + + return (sendCommand(packetCfg, maxWait)); //Wait for ack +} + +//Clear the antenna control settings using UBX-CFG-ANT +//This function is hopefully redundant but may be needed to release +//any PIO pins pre-allocated for antenna functions +boolean SFE_UBLOX_GPS::clearAntPIO(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_ANT; + packetCfg.len = 4; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0x10; // Antenna flag mask: set the recovery bit + payloadCfg[1] = 0; + payloadCfg[2] = 0xFF; // Antenna pin configuration: set pinSwitch and pinSCD to 31 + payloadCfg[3] = 0xFF; // Antenna pin configuration: set pinOCD to 31, set reconfig bit + + return (sendCommand(packetCfg, maxWait)); //Wait for ack +} + +//Returns the combined geofence state using UBX-NAV-GEOFENCE +boolean SFE_UBLOX_GPS::getGeofenceState(geofenceState ¤tGeofenceState, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_GEOFENCE; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(packetCfg, maxWait) == false) //Ask module for the geofence status. Loads into payloadCfg. + return (false); + + currentGeofenceState.status = payloadCfg[5]; // Extract the status + currentGeofenceState.numFences = payloadCfg[6]; // Extract the number of geofences + currentGeofenceState.combState = payloadCfg[7]; // Extract the combined state of all geofences + if (currentGeofenceState.numFences > 0) currentGeofenceState.states[0] = payloadCfg[8]; // Extract geofence 1 state + if (currentGeofenceState.numFences > 1) currentGeofenceState.states[1] = payloadCfg[10]; // Extract geofence 2 state + if (currentGeofenceState.numFences > 2) currentGeofenceState.states[2] = payloadCfg[12]; // Extract geofence 3 state + if (currentGeofenceState.numFences > 3) currentGeofenceState.states[3] = payloadCfg[14]; // Extract geofence 4 state + + return(true); +} + //Given a spot in the payload array, extract four bytes and build a long uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart) { diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 2c2292b..9d3c409 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -103,6 +103,10 @@ const uint8_t UBX_CFG_VALSET = 0x8A; //Used for config of higher version Ublox m const uint8_t UBX_CFG_VALGET = 0x8B; //Used for config of higher version Ublox modules (ie protocol v27 and above) const uint8_t UBX_CFG_VALDEL = 0x8C; //Used for config of higher version Ublox modules (ie protocol v27 and above) +const uint8_t UBX_CFG_GEOFENCE = 0x69; //Used to configure a geofence +const uint8_t UBX_CFG_ANT = 0x13; //Used to configure the antenna control settings +const uint8_t UBX_NAV_GEOFENCE = 0x39; //Used to poll the geofence status + const uint8_t UBX_CFG_TMODE3 = 0x71; //Used to enable Survey In Mode const uint8_t SVIN_MODE_DISABLE = 0x00; const uint8_t SVIN_MODE_ENABLE = 0x01; @@ -189,6 +193,22 @@ typedef struct boolean valid; //Goes true when both checksums pass } ubxPacket; +// Struct to hold the results returned by getGeofenceState (returned by UBX-NAV-GEOFENCE) +typedef struct { + uint8_t status; // Geofencing status: 0 - Geofencing not available or not reliable; 1 - Geofencing active + uint8_t numFences; // Number of geofences + uint8_t combState; // Combined (logical OR) state of all geofences: 0 - Unknown; 1 - Inside; 2 - Outside + uint8_t states[4]; // Geofence states: 0 - Unknown; 1 - Inside; 2 - Outside +} geofenceState; + +// Struct to hold the current geofence parameters +typedef struct { + uint8_t numFences; // Number of active geofences + int32_t lats[4]; // Latitudes of geofences (in degrees * 10^-7) + int32_t longs[4]; // Longitudes of geofences (in degrees * 10^-7) + uint32_t rads[4]; // Radii of geofences (in m * 10^-2) +} geofenceParams; + class SFE_UBLOX_GPS { public: @@ -319,6 +339,11 @@ class SFE_UBLOX_GPS void debugPrint(char *message); //Safely print debug statements void debugPrintln(char *message); //Safely print debug statements + //Support for geofences + boolean addGeofence(int32_t latitude, int32_t longitude, uint32_t radius, byte confidence = 0, byte pinPolarity = 0, byte pin = 0, uint16_t maxWait = 2000); // Add a new geofence + boolean clearGeofences(uint16_t maxWait = 2000); //Clears all geofences + boolean getGeofenceState(geofenceState ¤tGeofenceState, uint16_t maxWait = 2000); //Returns the combined geofence state + //Survey-in specific controls struct svinStructure { @@ -499,6 +524,10 @@ class SFE_UBLOX_GPS } highResModuleQueried; uint16_t rtcmLen = 0; + + //Support for geofences + boolean clearAntPIO(uint16_t maxWait = 2000); //Clears the antenna control pin settings to release the PIOs + geofenceParams currentGeofenceParams; // Global to store the geofence parameters }; #endif