Skip to content
This repository was archived by the owner on Jan 28, 2021. It is now read-only.

Commit 459f89b

Browse files
PaulZCPaulZC
committed
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..
1 parent 9268475 commit 459f89b

File tree

5 files changed

+353
-1
lines changed

5 files changed

+353
-1
lines changed

‎README.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ Thanks to:
3030
* [tve](https://github.com/tve) for building out serial additions and examples
3131
* [Redstoned](https://github.com/Redstoned) and [davidallenmann](https://github.com/davidallenmann) for adding PVT date and time
3232
* [wittend](https://forum.sparkfun.com/viewtopic.php?t=49874) for pointing out the RTCM print bug
33-
* Big thanks to [PaulZC](https://github.com/PaulZC) for implementing the combined key ValSet method
33+
* Big thanks to [PaulZC](https://github.com/PaulZC) for implementing the combined key ValSet method and geofence functions
3434
* [RollieRowland](https://github.com/RollieRowland) for adding HPPOSLLH (High Precision Geodetic Position)
3535
* [tedder](https://github.com/tedder) for moving iTOW to PVT instead of HPPOS and comment cleanup
3636

‎examples/Example18_Geofence/Example18_Geofence.ino

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
/*
2+
u-blox M8 geofence example
3+
4+
Written by Paul Clark (PaulZC)
5+
10th December 2019
6+
7+
License: MIT. See license file for more information but you can
8+
basically do whatever you want with this code.
9+
10+
This example demonstrates how to use the addGeofence and getGeofenceState functions
11+
12+
Feel like supporting open source hardware?
13+
Buy a board from SparkFun!
14+
ZED-F9P RTK2: https://www.sparkfun.com/products/15136
15+
NEO-M8P RTK: https://www.sparkfun.com/products/15005
16+
SAM-M8Q: https://www.sparkfun.com/products/15210
17+
ZOE-M8Q: https://www.sparkfun.com/products/15193
18+
19+
This example powers up the GPS and reads the fix.
20+
Once a valid 3D fix has been found, the code reads the latitude and longitude.
21+
The code then sets four geofences around that position with a radii of 5m, 10m, 15m and 20m with 95% confidence.
22+
The code then monitors the geofence status.
23+
The LED will be illuminated if you are inside the _combined_ geofence (i.e. within the 20m radius).
24+
25+
This code has been tested on the ZOE-M8Q.
26+
*/
27+
28+
#define LED LED_BUILTIN // Change this if your LED is on a different pin
29+
30+
#include <Wire.h> // Needed for I2C
31+
32+
#include "SparkFun_Ublox_Arduino_Library.h" //http://librarymanager/All#SparkFun_Ublox_GPS
33+
SFE_UBLOX_GPS myGPS;
34+
35+
void setup()
36+
{
37+
pinMode(LED, OUTPUT);
38+
39+
// Set up the I2C pins
40+
Wire.begin();
41+
42+
// Start the console serial port
43+
Serial.begin(115200);
44+
while (!Serial); // Wait for the user to open the serial monitor
45+
delay(100);
46+
Serial.println();
47+
Serial.println();
48+
Serial.println(F("u-blox M8 geofence example"));
49+
Serial.println();
50+
Serial.println();
51+
52+
delay(1000); // Let the GPS power up
53+
54+
if (myGPS.begin() == false) //Connect to the Ublox module using Wire port
55+
{
56+
Serial.println(F("Ublox GPS not detected at default I2C address. Please check wiring. Freezing."));
57+
while (1);
58+
}
59+
60+
//myGPS.enableDebugging(); // Enable debug messages
61+
myGPS.setI2COutput(COM_TYPE_UBX); // Limit I2C output to UBX (disable the NMEA noise)
62+
63+
Serial.println(F("Waiting for a 3D fix..."));
64+
65+
byte fixType = 0;
66+
67+
while (fixType != 3)
68+
{
69+
fixType = myGPS.getFixType(); // Get the fix type
70+
Serial.print(F("Fix: ")); // Print it
71+
Serial.print(fixType);
72+
if(fixType == 0) Serial.print(F(" = No fix"));
73+
else if(fixType == 1) Serial.print(F(" = Dead reckoning"));
74+
else if(fixType == 2) Serial.print(F(" = 2D"));
75+
else if(fixType == 3) Serial.print(F(" = 3D"));
76+
else if(fixType == 4) Serial.print(F(" = GNSS + Dead reckoning"));
77+
Serial.println();
78+
delay(1000);
79+
}
80+
81+
Serial.println(F("3D fix found!"));
82+
83+
long latitude = myGPS.getLatitude(); // Get the latitude in degrees * 10^-7
84+
Serial.print(F("Lat: "));
85+
Serial.print(latitude);
86+
87+
long longitude = myGPS.getLongitude(); // Get the longitude in degrees * 10^-7
88+
Serial.print(F(" Long: "));
89+
Serial.println(longitude);
90+
91+
uint32_t radius = 500; // Set the radius to 5m (radius is in m * 10^-2 i.e. cm)
92+
93+
byte confidence = 2; // Set the confidence level: 0=none, 1=68%, 2=95%, 3=99.7%, 4=99.99%
94+
95+
// Call clearGeofences() to clear all existing geofences.
96+
Serial.print(F("Clearing any existing geofences. clearGeofences returned: "));
97+
Serial.println(myGPS.clearGeofences());
98+
99+
// It is possible to define up to four geofences.
100+
// Call addGeofence up to four times to define them.
101+
Serial.println(F("Setting the geofences:"));
102+
103+
Serial.print(F("addGeofence for geofence 1 returned: "));
104+
Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence));
105+
106+
radius = 1000; // 10m
107+
Serial.print(F("addGeofence for geofence 2 returned: "));
108+
Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence));
109+
110+
radius = 1500; // 15m
111+
Serial.print(F("addGeofence for geofence 3 returned: "));
112+
Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence));
113+
114+
radius = 2000; // 20m
115+
Serial.print(F("addGeofence for geofence 4 returned: "));
116+
Serial.println(myGPS.addGeofence(latitude, longitude, radius, confidence));
117+
}
118+
119+
void loop()
120+
{
121+
geofenceState currentGeofenceState; // Create storage for the geofence state
122+
123+
boolean result = myGPS.getGeofenceState(currentGeofenceState);
124+
125+
Serial.print(F("getGeofenceState returned: ")); // Print the combined state
126+
Serial.print(result); // Get the geofence state
127+
128+
if (!result) // If getGeofenceState did not return true
129+
{
130+
Serial.println(F(".")); // Tidy up
131+
return; // and go round the loop again
132+
}
133+
134+
Serial.print(F(". status is: ")); // Print the status
135+
Serial.print(currentGeofenceState.status);
136+
137+
Serial.print(F(". numFences is: ")); // Print the numFences
138+
Serial.print(currentGeofenceState.numFences);
139+
140+
Serial.print(F(". combState is: ")); // Print the combined state
141+
Serial.print(currentGeofenceState.combState);
142+
143+
if (currentGeofenceState.combState == 0)
144+
{
145+
Serial.print(F(" = Unknown"));
146+
digitalWrite(LED, LOW);
147+
}
148+
if (currentGeofenceState.combState == 1)
149+
{
150+
Serial.print(F(" = Inside"));
151+
digitalWrite(LED, HIGH);
152+
}
153+
else if (currentGeofenceState.combState == 2)
154+
{
155+
Serial.print(F(" = Outside"));
156+
digitalWrite(LED, LOW);
157+
}
158+
159+
Serial.print(F(". The individual states are: ")); // Print the state of each geofence
160+
for(int i = 0; i < currentGeofenceState.numFences; i++)
161+
{
162+
if (i > 0) Serial.print(F(","));
163+
Serial.print(currentGeofenceState.states[i]);
164+
}
165+
Serial.println();
166+
167+
delay(1000);
168+
}

‎keywords.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -121,6 +121,10 @@ getGeoidSeparation KEYWORD2
121121
getHorizontalAccuracy KEYWORD2
122122
getVerticalAccuracy KEYWORD2
123123

124+
addGeofence KEYWORD2
125+
clearGeofences KEYWORD2
126+
getGeofenceState KEYWORD2
127+
124128
#######################################
125129
# Constants (LITERAL1)
126130
#######################################

‎src/SparkFun_Ublox_Arduino_Library.cpp

Lines changed: 151 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
SFE_UBLOX_GPS::SFE_UBLOX_GPS(void)
4242
{
4343
// Constructor
44+
currentGeofenceParams.numFences = 0; // Zero the number of geofences currently in use
4445
}
4546

4647
//Initialize the Serial port
@@ -1638,6 +1639,156 @@ boolean SFE_UBLOX_GPS::setAutoPVT(boolean enable, boolean implicitUpdate, uint16
16381639
return ok;
16391640
}
16401641

1642+
//Add a new geofence using UBX-CFG-GEOFENCE
1643+
boolean SFE_UBLOX_GPS::addGeofence(int32_t latitude, int32_t longitude, uint32_t radius, byte confidence, byte pinPolarity, byte pin, uint16_t maxWait)
1644+
{
1645+
if (currentGeofenceParams.numFences >= 4) return(false); // Quit if we already have four geofences defined
1646+
1647+
// Store the new geofence parameters
1648+
currentGeofenceParams.lats[currentGeofenceParams.numFences] = latitude;
1649+
currentGeofenceParams.longs[currentGeofenceParams.numFences] = longitude;
1650+
currentGeofenceParams.rads[currentGeofenceParams.numFences] = radius;
1651+
currentGeofenceParams.numFences = currentGeofenceParams.numFences + 1; // Increment the number of fences
1652+
1653+
packetCfg.cls = UBX_CLASS_CFG;
1654+
packetCfg.id = UBX_CFG_GEOFENCE;
1655+
packetCfg.len = (currentGeofenceParams.numFences * 12) + 8;
1656+
packetCfg.startingSpot = 0;
1657+
1658+
payloadCfg[0] = 0; // Message version = 0x00
1659+
payloadCfg[1] = currentGeofenceParams.numFences; // numFences
1660+
payloadCfg[2] = confidence; // confLvl = Confidence level 0-4 (none, 68%, 95%, 99.7%, 99.99%)
1661+
payloadCfg[3] = 0; // reserved1
1662+
if (pin > 0)
1663+
{
1664+
payloadCfg[4] = 1; // enable PIO combined fence state
1665+
}
1666+
else
1667+
{
1668+
payloadCfg[4] = 0; // disable PIO combined fence state
1669+
}
1670+
payloadCfg[5] = pinPolarity; // PIO pin polarity (0 = low means inside, 1 = low means outside (or unknown))
1671+
payloadCfg[6] = pin; // PIO pin
1672+
payloadCfg[7] = 0; //reserved2
1673+
payloadCfg[8] = currentGeofenceParams.lats[0] & 0xFF;
1674+
payloadCfg[9] = currentGeofenceParams.lats[0] >> 8;
1675+
payloadCfg[10] = currentGeofenceParams.lats[0] >> 16;
1676+
payloadCfg[11] = currentGeofenceParams.lats[0] >> 24;
1677+
payloadCfg[12] = currentGeofenceParams.longs[0] & 0xFF;
1678+
payloadCfg[13] = currentGeofenceParams.longs[0] >> 8;
1679+
payloadCfg[14] = currentGeofenceParams.longs[0] >> 16;
1680+
payloadCfg[15] = currentGeofenceParams.longs[0] >> 24;
1681+
payloadCfg[16] = currentGeofenceParams.rads[0] & 0xFF;
1682+
payloadCfg[17] = currentGeofenceParams.rads[0] >> 8;
1683+
payloadCfg[18] = currentGeofenceParams.rads[0] >> 16;
1684+
payloadCfg[19] = currentGeofenceParams.rads[0] >> 24;
1685+
if (currentGeofenceParams.numFences >= 2) {
1686+
payloadCfg[20] = currentGeofenceParams.lats[1] & 0xFF;
1687+
payloadCfg[21] = currentGeofenceParams.lats[1] >> 8;
1688+
payloadCfg[22] = currentGeofenceParams.lats[1] >> 16;
1689+
payloadCfg[23] = currentGeofenceParams.lats[1] >> 24;
1690+
payloadCfg[24] = currentGeofenceParams.longs[1] & 0xFF;
1691+
payloadCfg[25] = currentGeofenceParams.longs[1] >> 8;
1692+
payloadCfg[26] = currentGeofenceParams.longs[1] >> 16;
1693+
payloadCfg[27] = currentGeofenceParams.longs[1] >> 24;
1694+
payloadCfg[28] = currentGeofenceParams.rads[1] & 0xFF;
1695+
payloadCfg[29] = currentGeofenceParams.rads[1] >> 8;
1696+
payloadCfg[30] = currentGeofenceParams.rads[1] >> 16;
1697+
payloadCfg[31] = currentGeofenceParams.rads[1] >> 24;
1698+
}
1699+
if (currentGeofenceParams.numFences >= 3) {
1700+
payloadCfg[32] = currentGeofenceParams.lats[2] & 0xFF;
1701+
payloadCfg[33] = currentGeofenceParams.lats[2] >> 8;
1702+
payloadCfg[34] = currentGeofenceParams.lats[2] >> 16;
1703+
payloadCfg[35] = currentGeofenceParams.lats[2] >> 24;
1704+
payloadCfg[36] = currentGeofenceParams.longs[2] & 0xFF;
1705+
payloadCfg[37] = currentGeofenceParams.longs[2] >> 8;
1706+
payloadCfg[38] = currentGeofenceParams.longs[2] >> 16;
1707+
payloadCfg[39] = currentGeofenceParams.longs[2] >> 24;
1708+
payloadCfg[40] = currentGeofenceParams.rads[2] & 0xFF;
1709+
payloadCfg[41] = currentGeofenceParams.rads[2] >> 8;
1710+
payloadCfg[42] = currentGeofenceParams.rads[2] >> 16;
1711+
payloadCfg[43] = currentGeofenceParams.rads[2] >> 24;
1712+
}
1713+
if (currentGeofenceParams.numFences >= 4) {
1714+
payloadCfg[44] = currentGeofenceParams.lats[3] & 0xFF;
1715+
payloadCfg[45] = currentGeofenceParams.lats[3] >> 8;
1716+
payloadCfg[46] = currentGeofenceParams.lats[3] >> 16;
1717+
payloadCfg[47] = currentGeofenceParams.lats[3] >> 24;
1718+
payloadCfg[48] = currentGeofenceParams.longs[3] & 0xFF;
1719+
payloadCfg[49] = currentGeofenceParams.longs[3] >> 8;
1720+
payloadCfg[50] = currentGeofenceParams.longs[3] >> 16;
1721+
payloadCfg[51] = currentGeofenceParams.longs[3] >> 24;
1722+
payloadCfg[52] = currentGeofenceParams.rads[3] & 0xFF;
1723+
payloadCfg[53] = currentGeofenceParams.rads[3] >> 8;
1724+
payloadCfg[54] = currentGeofenceParams.rads[3] >> 16;
1725+
payloadCfg[55] = currentGeofenceParams.rads[3] >> 24;
1726+
}
1727+
return (sendCommand(packetCfg, maxWait)); //Wait for ack
1728+
}
1729+
1730+
//Clear all geofences using UBX-CFG-GEOFENCE
1731+
boolean SFE_UBLOX_GPS::clearGeofences(uint16_t maxWait)
1732+
{
1733+
packetCfg.cls = UBX_CLASS_CFG;
1734+
packetCfg.id = UBX_CFG_GEOFENCE;
1735+
packetCfg.len = 8;
1736+
packetCfg.startingSpot = 0;
1737+
1738+
payloadCfg[0] = 0; // Message version = 0x00
1739+
payloadCfg[1] = 0; // numFences
1740+
payloadCfg[2] = 0; // confLvl
1741+
payloadCfg[3] = 0; // reserved1
1742+
payloadCfg[4] = 0; // disable PIO combined fence state
1743+
payloadCfg[5] = 0; // PIO pin polarity (0 = low means inside, 1 = low means outside (or unknown))
1744+
payloadCfg[6] = 0; // PIO pin
1745+
payloadCfg[7] = 0; //reserved2
1746+
1747+
currentGeofenceParams.numFences = 0; // Zero the number of geofences currently in use
1748+
1749+
return (sendCommand(packetCfg, maxWait)); //Wait for ack
1750+
}
1751+
1752+
//Clear the antenna control settings using UBX-CFG-ANT
1753+
//This function is hopefully redundant but may be needed to release
1754+
//any PIO pins pre-allocated for antenna functions
1755+
boolean SFE_UBLOX_GPS::clearAntPIO(uint16_t maxWait)
1756+
{
1757+
packetCfg.cls = UBX_CLASS_CFG;
1758+
packetCfg.id = UBX_CFG_ANT;
1759+
packetCfg.len = 4;
1760+
packetCfg.startingSpot = 0;
1761+
1762+
payloadCfg[0] = 0x10; // Antenna flag mask: set the recovery bit
1763+
payloadCfg[1] = 0;
1764+
payloadCfg[2] = 0xFF; // Antenna pin configuration: set pinSwitch and pinSCD to 31
1765+
payloadCfg[3] = 0xFF; // Antenna pin configuration: set pinOCD to 31, set reconfig bit
1766+
1767+
return (sendCommand(packetCfg, maxWait)); //Wait for ack
1768+
}
1769+
1770+
//Returns the combined geofence state using UBX-NAV-GEOFENCE
1771+
boolean SFE_UBLOX_GPS::getGeofenceState(geofenceState &currentGeofenceState, uint16_t maxWait)
1772+
{
1773+
packetCfg.cls = UBX_CLASS_NAV;
1774+
packetCfg.id = UBX_NAV_GEOFENCE;
1775+
packetCfg.len = 0;
1776+
packetCfg.startingSpot = 0;
1777+
1778+
if (sendCommand(packetCfg, maxWait) == false) //Ask module for the geofence status. Loads into payloadCfg.
1779+
return (false);
1780+
1781+
currentGeofenceState.status = payloadCfg[5]; // Extract the status
1782+
currentGeofenceState.numFences = payloadCfg[6]; // Extract the number of geofences
1783+
currentGeofenceState.combState = payloadCfg[7]; // Extract the combined state of all geofences
1784+
if (currentGeofenceState.numFences > 0) currentGeofenceState.states[0] = payloadCfg[8]; // Extract geofence 1 state
1785+
if (currentGeofenceState.numFences > 1) currentGeofenceState.states[1] = payloadCfg[10]; // Extract geofence 2 state
1786+
if (currentGeofenceState.numFences > 2) currentGeofenceState.states[2] = payloadCfg[12]; // Extract geofence 3 state
1787+
if (currentGeofenceState.numFences > 3) currentGeofenceState.states[3] = payloadCfg[14]; // Extract geofence 4 state
1788+
1789+
return(true);
1790+
}
1791+
16411792
//Given a spot in the payload array, extract four bytes and build a long
16421793
uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart)
16431794
{

0 commit comments

Comments
 (0)