diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 11:30:02 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 11:30:02 +0200 |
commit | 81adc52671d920ffe184948267fcc1f9fbb027cc (patch) | |
tree | 48d7dcdd387dd3c5457dc81709c04426eb2b8f71 /src/modules/navigator | |
parent | 8262739b6219f54e7ea31de93cd81304df311ab9 (diff) | |
download | px4-firmware-81adc52671d920ffe184948267fcc1f9fbb027cc.tar.gz px4-firmware-81adc52671d920ffe184948267fcc1f9fbb027cc.tar.bz2 px4-firmware-81adc52671d920ffe184948267fcc1f9fbb027cc.zip |
geofence: add counter threshold for subsequent detections
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/geofence.cpp | 22 | ||||
-rw-r--r-- | src/modules/navigator/geofence.h | 8 | ||||
-rw-r--r-- | src/modules/navigator/geofence_params.c | 11 |
3 files changed, 38 insertions, 3 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4a02a0c3f..a3805dc0f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -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(); @@ -108,6 +110,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) return true; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 91c74572e..65ebb0c3d 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -77,9 +77,10 @@ public: */ bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); - bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + bool inside(double lat, double lon, float altitude); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -97,7 +98,7 @@ public: bool isEmpty() {return _verticesCount == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } - + int getSource() { return _param_source.get(); } private: @@ -112,6 +113,9 @@ private: control::BlockParamInt _param_geofence_on; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; + control::BlockParamInt _param_counter_threshold; + + uint8_t _outside_counter; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 32902ee97..fca3918e1 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -83,3 +83,14 @@ PARAM_DEFINE_INT32(GF_ALTMODE, 0); * @group Geofence */ PARAM_DEFINE_INT32(GF_SOURCE, 0); + +/** + * Geofence counter limit + * + * Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered + * + * @min -1 + * @max 10 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_COUNT, -1); |