fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

geofence: add counter threshold for subsequent detections

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-24 11:30:02 +02:00
parent 8262739b62
commit 81adc52671
3 changed files with 38 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+21 -1
View File
@@ -64,7 +64,9 @@ Geofence::Geofence() :
_verticesCount(0),
_param_geofence_on(this, "ON"),
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE")
_param_source(this, "SOURCE"),
_param_counter_threshold(this, "COUNT"),
_outside_counter(0)
{
/* Load initial params */
updateParams();
@@ -107,6 +109,24 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
}
bool Geofence::inside(double lat, double lon, float altitude)
{
bool inside_fence = inside_polygon(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
return inside_fence;
} {
_outside_counter++;
if(_outside_counter > _param_counter_threshold.get()) {
return inside_fence;
} else {
return true;
}
}
}
bool Geofence::inside_polygon(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */
if (_param_geofence_on.get() != 1)