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

Commit ab0bf7a

Browse files
nseidlenseidle
authored
Merge pull request #50 from PaulZC/PaulZC_geofence
Added the geofence functions
2 parents 877eb4e + 459f89b commit ab0bf7a

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
@@ -32,7 +32,7 @@ Thanks to:
3232
* [tve](https://github.com/tve) for building out serial additions and examples
3333
* [Redstoned](https://github.com/Redstoned) and [davidallenmann](https://github.com/davidallenmann) for adding PVT date and time
3434
* [wittend](https://forum.sparkfun.com/viewtopic.php?t=49874) for pointing out the RTCM print bug
35-
* Big thanks to [PaulZC](https://github.com/PaulZC) for implementing the combined key ValSet method
35+
* Big thanks to [PaulZC](https://github.com/PaulZC) for implementing the combined key ValSet method and geofence functions
3636
* [RollieRowland](https://github.com/RollieRowland) for adding HPPOSLLH (High Precision Geodetic Position)
3737
* [tedder](https://github.com/tedder) for moving iTOW to PVT instead of HPPOS and comment cleanup
3838

‎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)