Added the geofence functions

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
 

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 <Wire.h> // 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);
+}

keywords.txt

@@ -121,6 +121,10 @@ getGeoidSeparation KEYWORD2
 getHorizontalAccuracy KEYWORD2
 getVerticalAccuracy KEYWORD2
 
+addGeofence	KEYWORD2
+clearGeofences	KEYWORD2
+getGeofenceState	KEYWORD2
+
 #######################################
 # Constants (LITERAL1)
 #######################################

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 &currentGeofenceState, 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)
 {