From 25dfd84b40177d40a5848f6c549f5d326f670bee Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 22 Feb 2015 23:04:23 -0500 Subject: Geofence max horizontal and vertical distance -changes GF_ON to GF_MODE -adds GF_MAX_HOR_DIST and GF_MAX_VER_DIST params --- src/modules/navigator/geofence.cpp | 53 +++++++++++++++++++++++++++++--- src/modules/navigator/geofence.h | 12 ++++++-- src/modules/navigator/geofence_params.c | 27 +++++++++++++--- src/modules/navigator/navigator.h | 2 ++ src/modules/navigator/navigator_main.cpp | 11 +++++-- 5 files changed, 91 insertions(+), 14 deletions(-) (limited to 'src/modules/navigator') 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 #include #include #include @@ -49,6 +50,12 @@ #include #include #include +#include + +#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; } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 37a41b68a..effce9e97 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -76,7 +77,9 @@ public: * @return true: system is inside fence, false: system is outside fence */ bool 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); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -103,16 +106,21 @@ public: private: orb_advert_t _fence_pub; /**< publish fence topic */ + home_position_s _home_pos; + bool _home_pos_set; + float _altitude_min; float _altitude_max; unsigned _verticesCount; /* Params */ - control::BlockParamInt _param_geofence_on; + control::BlockParamInt _param_geofence_mode; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; control::BlockParamInt _param_counter_threshold; + control::BlockParamInt _param_max_hor_distance; + control::BlockParamInt _param_max_ver_distance; uint8_t _outside_counter; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index fca3918e1..f3e7d4c84 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -48,16 +48,15 @@ */ /** - * Enable geofence. + * Geofence mode. * - * Set to 1 to enable geofence. - * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both * * @min 0 - * @max 1 + * @max 3 * @group Geofence */ -PARAM_DEFINE_INT32(GF_ON, 1); +PARAM_DEFINE_INT32(GF_MODE, 0); /** * Geofence altitude mode @@ -94,3 +93,21 @@ PARAM_DEFINE_INT32(GF_SOURCE, 0); * @group Geofence */ PARAM_DEFINE_INT32(GF_COUNT, -1); + +/** + * Max horizontal distance in meters. + * + * Set to > 0 to activate RTL if horizontal distance to home exceeds this value. + * + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); + +/** + * Max vertical distance in meters. + * + * Set to > 0 to activate RTL if vertical distance to home exceeds this value. + * + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_MAX_VER_DIST, -1); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d9d911d9c..d2acce789 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -186,6 +186,8 @@ private: geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; + bool _home_position_set; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ad2349c94..05fac7b81 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -123,6 +123,7 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, + _home_position_set(false), _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -203,7 +204,13 @@ Navigator::sensor_combined_update() void Navigator::home_position_update() { - orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + bool updated = false; + orb_check(_home_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + _home_position_set = true; + } } void @@ -392,7 +399,7 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { -- cgit v1.2.3