diff options
Diffstat (limited to 'src/modules/navigator/geofence.cpp')
-rw-r--r-- | src/modules/navigator/geofence.cpp | 53 |
1 files changed, 48 insertions, 5 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 9bc9be245..efbc2689b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -39,6 +39,7 @@ */ #include "geofence.h" +#include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_global_position.h> #include <string.h> #include <dataman/dataman.h> @@ -49,6 +50,12 @@ #include <nuttx/config.h> #include <unistd.h> #include <mavlink/mavlink_log.h> +#include <geo/geo.h> + +#define GEOFENCE_OFF 0 +#define GEOFENCE_FILE_ONLY 1 +#define GEOFENCE_MAX_DISTANCES_ONLY 2 +#define GEOFENCE_FILE_AND_MAX_DISTANCES 3 /* Oddly, ERROR is not defined for C++ */ @@ -60,13 +67,17 @@ static const int ERROR = -1; Geofence::Geofence() : SuperBlock(NULL, "GF"), _fence_pub(-1), + _home_pos{}, + _home_pos_set(false), _altitude_min(0), _altitude_max(0), _verticesCount(0), - _param_geofence_on(this, "ON"), + _param_geofence_mode(this, "MODE"), _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), + _param_max_hor_distance(this, "MAX_HOR_DIST"), + _param_max_ver_distance(this, "MAX_VER_DIST"), _outside_counter(0), _mavlinkFd(-1) { @@ -92,10 +103,14 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, + const struct home_position_s home_pos, bool home_position_set) { updateParams(); + _home_pos = home_pos; + _home_pos_set = home_position_set; + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); @@ -118,13 +133,40 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, bool Geofence::inside(double lat, double lon, float altitude) { + if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) { + int32_t max_horizontal_distance = _param_max_hor_distance.get(); + int32_t max_vertical_distance = _param_max_ver_distance.get(); + + if (max_horizontal_distance > 0 || max_vertical_distance > 0) { + if (_home_pos_set) { + float dist_xy = -1.0f; + float dist_z = -1.0f; + get_distance_to_point_global_wgs84(lat, lon, altitude, + _home_pos.lat, _home_pos.lon, _home_pos.alt, + &dist_xy, &dist_z); + + if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { + mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max vertical distance by %.0f m", + (double)(dist_z - max_vertical_distance)); + return false; + } + + if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { + mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max horizontal distance by %.0f m", + (double)(dist_xy - max_horizontal_distance)); + return false; + } + } + } + } + bool inside_fence = inside_polygon(lat, lon, altitude); if (inside_fence) { _outside_counter = 0; return inside_fence; - } { + } else { _outside_counter++; if (_outside_counter > _param_counter_threshold.get()) { @@ -139,8 +181,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { - /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) { + /* Return true if geofence is disabled or only checking max distances */ + if ((_param_geofence_mode.get() == GEOFENCE_OFF) + || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) { return true; } |