diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index b697b7085b..44945afaea 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -43,7 +43,7 @@ bool battery_low_remaining_time # Low battery based on remaining flight ti bool battery_unhealthy # Battery unhealthy # Other -bool primary_geofence_breached # Primary Geofence breached +bool geofence_breached # Geofence breached (one or multiple) bool mission_failure # Mission failure bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) bool wind_limit_exceeded # Wind limit exceeded diff --git a/msg/GeofenceResult.msg b/msg/GeofenceResult.msg index 838f3b39cc..7782d1d6e8 100644 --- a/msg/GeofenceResult.msg +++ b/msg/GeofenceResult.msg @@ -6,7 +6,8 @@ uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL uint8 GF_ACTION_TERMINATE = 4 # flight termination uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND -uint8 geofence_violation_reason # one of geofence_violation_reason_t::* +bool geofence_max_dist_triggered # true the check for max distance from Home is triggered +bool geofence_max_alt_triggered # true the check for max altitude above Home is triggered +bool geofence_custom_fence_triggered # true the check for custom inclusion/exclusion geofence(s) is triggered -bool primary_geofence_breached # true if the primary geofence is breached -uint8 primary_geofence_action # action to take when the primary geofence is breached +uint8 geofence_action # action to take when the geofence is breached diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index cf8ff346f7..af0969b87f 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -733,24 +733,6 @@ "description": "Reduce throttle!" } } - }, - "geofence_violation_reason_t": { - "type": "uint8_t", - "description": "Reason for geofence violation", - "entries": { - "0": { - "name": "dist_to_home_exceeded", - "description": "maximum distance to home exceeded" - }, - "1": { - "name": "max_altitude_exceeded", - "description": "maximum altitude exceeded" - }, - "2": { - "name": "fence_violation", - "description": "approaching or outside geofence" - } - } } }, "navigation_mode_groups": { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp index ecb3324ab8..a519856202 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp @@ -41,26 +41,64 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter) geofence_result = {}; } - reporter.failsafeFlags().primary_geofence_breached = geofence_result.primary_geofence_breached; + const bool any_geofence_triggered = geofence_result.geofence_max_dist_triggered || + geofence_result.geofence_max_alt_triggered || + geofence_result.geofence_custom_fence_triggered; - if (geofence_result.primary_geofence_action != 0 && reporter.failsafeFlags().primary_geofence_breached) { - /* EVENT - * @description - * - * This check can be configured via GF_ACTION parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, - events::ID("check_gf_violation"), - events::Log::Error, "Geofence violation: {1}", - (events::px4::enums::geofence_violation_reason_t)geofence_result.geofence_violation_reason); + reporter.failsafeFlags().geofence_breached = any_geofence_triggered; - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Geofence violation"); + if (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE && any_geofence_triggered) { + + if (geofence_result.geofence_max_dist_triggered) { + /* EVENT + * @description + * + * This check can be configured via GF_ACTION and GF_MAX_HOR_DIST parameters. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_gf_violation_max_hor_dist"), + events::Log::Error, "Geofence violation: exceeding maximum distance to Home"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Geofence violation: exceeding maximum distance to Home"); + } + } + + if (geofence_result.geofence_max_alt_triggered) { + /* EVENT + * @description + * + * This check can be configured via GF_ACTION and GF_MAX_VER_DIST parameters. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_gf_violation_max_alt"), + events::Log::Error, "Geofence violation: exceeding maximum altitude above Home"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Geofence violation: exceeding maximum altitude above Home"); + } + } + + if (geofence_result.geofence_custom_fence_triggered) { + /* EVENT + * @description + * + * This check can be configured via GF_ACTION parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_gf_violation_custom_gf"), + events::Log::Error, "Geofence violation: approaching or outside geofence"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Geofence violation: approaching or outside geofence"); + } } } - if (geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_RTL + if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL && reporter.failsafeFlags().home_position_invalid) { /* EVENT * @description diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index b5da288fe5..61cbff2beb 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -442,7 +442,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL)); } - CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); + CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery CHECK_FAILSAFE(status_flags, battery_low_remaining_time, diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index c96d0b6cb9..c349340ac3 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -339,7 +339,8 @@ private: geofence_result_s geofence; if (_geofence_sub.update(&geofence)) { - if (geofence.primary_geofence_breached) { + if (geofence.geofence_max_dist_triggered || geofence.geofence_max_alt_triggered + || geofence.geofence_custom_fence_triggered) { msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE; } diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 8cc2c2e9e3..993801c2f1 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -51,8 +51,6 @@ #include "navigator.h" -#define GEOFENCE_RANGE_WARNING_LIMIT 5000000 - Geofence::Geofence(Navigator *navigator) : ModuleParams(navigator), _navigator(navigator) @@ -256,31 +254,15 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) { bool inside_fence = true; - if (isHomeRequired() && _navigator->home_global_position_valid()) { - - const float max_horizontal_distance = _param_gf_max_hor_dist.get(); - - const double home_lat = _navigator->get_home_position()->lat; - const double home_lon = _navigator->get_home_position()->lon; - const float home_alt = _navigator->get_home_position()->alt; + if (_param_gf_max_hor_dist.get() > FLT_EPSILON && _navigator->home_global_position_valid()) { float dist_xy = -1.0f; float dist_z = -1.0f; - get_distance_to_point_global_wgs84(lat, lon, altitude, home_lat, home_lon, home_alt, &dist_xy, &dist_z); + get_distance_to_point_global_wgs84(lat, lon, altitude, _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon, _navigator->get_home_position()->alt, &dist_xy, &dist_z); - if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) { - if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)\t", - (double)max_horizontal_distance); - events::send(events::ID("navigator_geofence_max_dist_from_home"), {events::Log::Critical, events::LogInternal::Warning}, - "Geofence: maximum distance from home reached ({1:.0m})", - max_horizontal_distance); - _last_horizontal_range_warning = hrt_absolute_time(); - } - - inside_fence = false; - } + inside_fence = dist_xy < _param_gf_max_hor_dist.get(); } return inside_fence; @@ -290,25 +272,10 @@ bool Geofence::isBelowMaxAltitude(float altitude) { bool inside_fence = true; - if (isHomeRequired() && _navigator->home_alt_valid()) { + if (_param_gf_max_ver_dist.get() > FLT_EPSILON && _navigator->home_alt_valid()) { - const float max_vertical_distance = _param_gf_max_ver_dist.get(); - const float home_alt = _navigator->get_home_position()->alt; - - float dist_z = altitude - home_alt; - - if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) { - if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)\t", - (double)max_vertical_distance); - events::send(events::ID("navigator_geofence_max_alt_from_home"), {events::Log::Critical, events::LogInternal::Warning}, - "Geofence: maximum altitude above home reached ({1:.0m_v})", - max_vertical_distance); - _last_vertical_range_warning = hrt_absolute_time(); - } - - inside_fence = false; - } + const float dist_z = altitude - _navigator->get_home_position()->alt; + inside_fence = dist_z < _param_gf_max_ver_dist.get(); } return inside_fence; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 5b9f755d9b..08c7d9aa38 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -81,8 +81,29 @@ public: */ void updateFence(); + + /** + * Check if a 3D point passes the Geofence test. + * Checks max distance, max altitude, inside polygon or circle. + * In addition to checkPolygons(), this takes all additional parameters into account. + * + * @return false for a geofence violation + */ + bool checkPointAgainstAllGeofences(double lat, double lon, float altitude); + + /** + * @brief check if the horizontal distance to Home is greater than the maximum allowed distance + * + * @return true if the horizontal distance to Home is smaller than the maximum allowed distance + */ bool isCloserThanMaxDistToHome(double lat, double lon, float altitude); + + /** + * @brief check if the altitude above Home is greater than the maximum allowed altitude + * + * @return true if the altitude above Home is smaller than the maximum allowed altitude + */ bool isBelowMaxAltitude(float altitude); virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude); @@ -118,7 +139,6 @@ public: int getGeofenceAction() { return _param_gf_action.get(); } float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); } - float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); } bool getPredict() { return _param_gf_predict.get(); } bool isHomeRequired(); @@ -156,9 +176,6 @@ private: DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4}; DatamanClient &_dataman_client = _dataman_cache.client(); - hrt_abstime _last_horizontal_range_warning{0}; - hrt_abstime _last_vertical_range_warning{0}; - float _altitude_min{0.0f}; float _altitude_max{0.0f}; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index cbca819644..a775405282 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -76,12 +76,11 @@ PARAM_DEFINE_INT32(GF_ACTION, 2); */ PARAM_DEFINE_INT32(GF_SOURCE, 0); - - /** - * Max horizontal distance in meters. + * Max horizontal distance from Home * - * Maximum horizontal distance in meters the vehicle can be from home before triggering a geofence action. Disabled if 0. + * Maximum horizontal distance in meters the vehicle can be from Home before triggering a geofence action. + * Disabled if 0. * * @unit m * @min 0 @@ -89,12 +88,13 @@ PARAM_DEFINE_INT32(GF_SOURCE, 0); * @increment 1 * @group Geofence */ -PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0); +PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0.0f); /** - * Max vertical distance in meters. + * Max vertical distance from Home * - * Maximum vertical distance in meters the vehicle can be from home before triggering a geofence action. Disabled if 0. + * Maximum vertical distance in meters the vehicle can be from Home before triggering a geofence action. + * Disabled if 0. * * @unit m * @min 0 @@ -102,7 +102,7 @@ PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0); * @increment 1 * @group Geofence */ -PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0); +PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0.0f); /** * [EXPERIMENTAL] Use Pre-emptive geofence triggering diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 675cf8c286..640ec54ba8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -961,58 +961,58 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) } } + double current_latitude = _global_pos.lat; + double current_longitude = _global_pos.lon; + float current_altitude = _global_pos.alt; + bool position_valid = _global_pos.timestamp > 0; + + if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { + current_latitude = _gps_pos.latitude_deg; + current_longitude = _gps_pos.longitude_deg; + current_altitude = _gps_pos.altitude_msl_m; + position_valid = _global_pos.timestamp > 0; + } + _gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance); _gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance); _gf_breach_avoidance.setTestPointBearing(test_point_bearing); - _gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt); + _gf_breach_avoidance.setCurrentPosition(current_latitude, current_longitude, current_altitude); _gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome()); if (home_global_position_valid()) { _gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt); } + double test_point_latitude = current_latitude; + double test_point_longitude = current_longitude; + float test_point_altitude = current_altitude; + if (_geofence.getPredict()) { - fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint(); + matrix::Vector2fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint(); + test_point_latitude = fence_violation_test_point(0); + test_point_longitude = fence_violation_test_point(1); + test_point_altitude = current_altitude + vertical_test_point_distance; + } } else { fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon); vertical_test_point_distance = 0; } - gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0), - fence_violation_test_point(1), - _global_pos.alt); - - gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt + - vertical_test_point_distance); - - gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0), - fence_violation_test_point(1), - _global_pos.alt); + _geofence_result.geofence_max_dist_triggered = !_geofence.isCloserThanMaxDistToHome(test_point_latitude, + test_point_longitude, test_point_altitude); + _geofence_result.geofence_max_alt_triggered = !_geofence.isBelowMaxAltitude(test_point_altitude); + _geofence_result.geofence_custom_fence_triggered = !_geofence.isInsidePolygonOrCircle(test_point_latitude, + test_point_longitude, test_point_altitude); _last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.timestamp = hrt_absolute_time(); - _geofence_result.primary_geofence_action = _geofence.getGeofenceAction(); + _geofence_result.geofence_action = _geofence.getGeofenceAction(); - - if (gf_violation_type.value) { - /* inform other apps via the mission result */ - _geofence_result.primary_geofence_breached = true; - - using geofence_violation_reason_t = events::px4::enums::geofence_violation_reason_t; - - if (gf_violation_type.flags.fence_violation) { - _geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::fence_violation; - - } else if (gf_violation_type.flags.max_altitude_exceeded) { - _geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::max_altitude_exceeded; - - } else if (gf_violation_type.flags.dist_to_home_exceeded) { - _geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::dist_to_home_exceeded; - - } + if (_geofence_result.geofence_max_dist_triggered || _geofence_result.geofence_max_alt_triggered || + _geofence_result.geofence_custom_fence_triggered) { /* Issue a warning about the geofence violation once and only if we are armed */ if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { @@ -1060,8 +1060,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) } } else { - /* inform other apps via the mission result */ - _geofence_result.primary_geofence_breached = false; /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false;