forked from Archive/PX4-Autopilot
geofence: add counter threshold for subsequent detections
This commit is contained in:
parent
8262739b62
commit
81adc52671
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue