From ece74e8d24051776408a39960b99ecba3c81ebe1 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 5 Jun 2026 10:47:35 +0300 Subject: [PATCH] refactor(navigator): Removed geofence breach avoidance feature (#27465) * refactor(navigator): removed geofence breach avoidance library - was disabled by default as an experimental feature - is too unpredictable given that it commands reposition points, which could be arbitrarily close to objects - does not work well in space constrained scenarios Signed-off-by: RomanBapst * fix(docs): removed reference to GF_PREDICT Signed-off-by: RomanBapst * style(navigator): fix formatting * docs(safety): add minimal guidelines on how to strictly avoid geofence breaches * docs(safety): slightly simplify wording * docs(safety): revert typo * docs(docs): Tweak --------- Signed-off-by: RomanBapst Co-authored-by: Balduin Co-authored-by: Hamish Willee --- docs/en/config/safety.md | 18 +- src/modules/navigator/CMakeLists.txt | 2 - .../GeofenceBreachAvoidance/CMakeLists.txt | 39 --- .../GeofenceBreachAvoidanceTest.cpp | 297 ------------------ .../GeofenceBreachAvoidance/fake_geofence.hpp | 136 -------- .../geofence_breach_avoidance.cpp | 293 ----------------- .../geofence_breach_avoidance.h | 144 --------- src/modules/navigator/geofence.h | 4 +- src/modules/navigator/geofence_params.yaml | 11 - src/modules/navigator/navigator.h | 7 +- src/modules/navigator/navigator_main.cpp | 130 ++------ 11 files changed, 43 insertions(+), 1038 deletions(-) delete mode 100644 src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt delete mode 100644 src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp delete mode 100644 src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp delete mode 100644 src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp delete mode 100644 src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h diff --git a/docs/en/config/safety.md b/docs/en/config/safety.md index fcffc11c9a8..a103f740b38 100644 --- a/docs/en/config/safety.md +++ b/docs/en/config/safety.md @@ -152,6 +152,15 @@ The _Geofence Failsafe_ is triggered when the drone breaches a "virtual" perimet In its simplest form, the perimeter is set up as a cylinder centered around the home position. If the vehicle moves outside the radius or above the altitude the specified _Failsafe Action_ will trigger. +Note that the failsafe action will only trigger once the vehicle has already breached the geofence. +If you have a strict no-fly zone for safety or legal reasons, set [GF_ACTION](../advanced_config/parameter_reference.md#GF_ACTION) to `Hold` and include a safety margin in your geofences. +The margin should be at least: + +- **Fixed-Wing**: The loiter radius [NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD). +- **Multicopter**: The stopping distance (`v^2 / 2a` with `v`=[MPC_XY_VEL_MAX](../advanced_config/parameter_reference.md#MPC_XY_VEL_MAX) and `a`=[MPC_ACC_HOR_MAX](../advanced_config/parameter_reference.md#MPC_ACC_HOR_MAX)). + +Use a margin above those nominal values to account for possible tailwind, position uncertainty, attitude tracking delay, etc. + ![Safety - Geofence (QGC)](../../assets/qgc/setup/safety/safety_geofence.png) :::tip @@ -173,11 +182,10 @@ Due to the inherent danger of this, this function is disabled using [CBRK_FLIGHT The following settings also apply, but are not displayed in the QGC UI. -| Setting | Parameter | Description | -| ------------------------------------------------------------------ | ---------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------- | -| Geofence source | [GF_SOURCE](../advanced_config/parameter_reference.md#GF_SOURCE) | Set whether position source is estimated global position or direct from the GPS device. | -| Preemptive geofence triggering | [GF_PREDICT](../advanced_config/parameter_reference.md#GF_PREDICT) | (Experimental) Trigger geofence if current motion of the vehicle is predicted to trigger the breach (rather than late triggering after the breach). | -| Circuit breaker for flight termination | [CBRK_FLIGHTTERM](../advanced_config/parameter_reference.md#CBRK_FLIGHTTERM) | Enables/Disables flight termination action (disabled by default). | +| Setting | Parameter | Description | +| ------------------------------------------------------------------ | ---------------------------------------------------------------------------- | --------------------------------------------------------------------------------------- | +| Geofence source | [GF_SOURCE](../advanced_config/parameter_reference.md#GF_SOURCE) | Set whether position source is estimated global position or direct from the GPS device. | +| Circuit breaker for flight termination | [CBRK_FLIGHTTERM](../advanced_config/parameter_reference.md#CBRK_FLIGHTTERM) | Enables/Disables flight termination action (disabled by default). | ## Position Estimation Failsafes diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 6c160f99e65..ae2f8807959 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -31,7 +31,6 @@ # ############################################################################ -add_subdirectory(GeofenceBreachAvoidance) add_subdirectory(MissionFeasibility) if(BUILD_TESTING) @@ -78,7 +77,6 @@ px4_add_module( dataman_client geo adsb - geofence_breach_avoidance motion_planning mission_feasibility_checker rtl_time_estimator diff --git a/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt b/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt deleted file mode 100644 index 7be0cfc4caf..00000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(geofence_breach_avoidance - geofence_breach_avoidance.cpp - geofence_breach_avoidance.h -) - -px4_add_functional_gtest(SRC GeofenceBreachAvoidanceTest.cpp LINKLIBS geofence_breach_avoidance modules__navigator motion_planning) diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp deleted file mode 100644 index 4441a251662..00000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp +++ /dev/null @@ -1,297 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include "geofence_breach_avoidance.h" -#include "fake_geofence.hpp" -#include - -using namespace matrix; - -class GeofenceBreachAvoidanceTest : public ::testing::Test -{ -public: - void SetUp() override - { - param_control_autosave(false); - } -}; - -TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance) -{ - - GeofenceBreachAvoidance gf_avoidance(nullptr); - Vector2d home_global(42.1, 8.2); - - MapProjection ref{home_global(0), home_global(1)}; - - Vector2f waypoint_north_east_local(1.0, 1.0); - waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5; - Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5); - - Vector2f waypoint_north_east_reprojected = ref.project(waypoint_north_east_global(0), waypoint_north_east_global(1)); - - EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0)); - EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1)); - - - Vector2f waypoint_south_west_local = -waypoint_north_east_local; - - Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5); - - Vector2f waypoint_south_west_reprojected = ref.project(waypoint_southwest_global(0), waypoint_southwest_global(1)); - - EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0)); - EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1)); - - - Vector2d same_as_home_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 0.0); - - EXPECT_LT(Vector2d(home_global - same_as_home_global).norm(), 1e-4); -} - -TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing) -{ - GeofenceBreachAvoidance gf_avoidance(nullptr); - FakeGeofence geo; - Vector2d home_global(42.1, 8.2); - MapProjection ref{home_global(0), home_global(1)}; - - geofence_violation_type_u gf_violation{}; - gf_violation.flags.fence_violation = true; - - gf_avoidance.setHorizontalTestPointDistance(20.0f); - gf_avoidance.setTestPointBearing(0.0f); - gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0); - - - Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); - - // the expected loiter point is located test_point_distance behind - Vector2d loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, -20.0f); - - EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0)); - EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); - - - geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE); - loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); - - loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, -M_PI_F * 0.5f, 20.0f); - - EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0)); - EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); - - geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::RIGHT_INSIDE_LEFT_OUTSIDE); - loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); - - loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.5f, 20.0f); - - EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0)); - EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); - - gf_violation.flags.fence_violation = false; - loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); - - EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), home_global(0)); - EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), home_global(1)); -} - -TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) -{ - GeofenceBreachAvoidance gf_avoidance(nullptr); - FakeGeofence geo; - Vector2d home_global(42.1, 8.2); - MapProjection ref{42.1, 8.2}; - - param_t param = param_handle(px4::params::MPC_ACC_HOR); - - float value = 3; - param_set(param, &value); - - param = param_handle(px4::params::MPC_ACC_HOR_MAX); - value = 5; - param_set(param, &value); - - param = param_handle(px4::params::MPC_JERK_AUTO); - value = 8; - param_set(param, &value); - - geofence_violation_type_u gf_violation{}; - gf_violation.flags.fence_violation = true; - - gf_avoidance.setHorizontalTestPointDistance(30.0f); - gf_avoidance.setTestPointBearing(0.0f); - gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0); - // vehicle is approaching the fence at a crazy velocity - gf_avoidance.setHorizontalVelocity(1000.0f); - gf_avoidance.computeBrakingDistanceMultirotor(); - - geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::GF_BOUNDARY_20M_AHEAD); - - Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); - - Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, - 20.0f - gf_avoidance.getMinHorDistToFenceMulticopter()); - - float error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0), - loiter_point_predicted(1)); - - EXPECT_LE(error, 0.5f); - - // vehicle is approaching fenc slowly, plenty of time to brake - gf_avoidance.setHorizontalVelocity(0.1f); - gf_avoidance.computeBrakingDistanceMultirotor(); - - loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); - loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, - gf_avoidance.computeBrakingDistanceMultirotor()); - - - error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0), - loiter_point_predicted(1)); - - EXPECT_LE(error, 0.0f); - - gf_violation.flags.fence_violation = false; - loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); - - EXPECT_LT(Vector2d(loiter_point - home_global).norm(), 1e-4); -} - -TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) -{ - GeofenceBreachAvoidance gf_avoidance(nullptr); - const float current_alt_amsl = 100.0f; - const float vertical_test_point_dist = 10.0f; - - gf_avoidance.setVerticalTestPointDistance(vertical_test_point_dist); - gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); // just care about altitude - geofence_violation_type_u gf_violation{}; - gf_violation.flags.max_altitude_exceeded = true; - - float loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation); - - EXPECT_EQ(loiter_alt, current_alt_amsl - 2 * vertical_test_point_dist); - - gf_violation.flags.max_altitude_exceeded = false; - - loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation); - - EXPECT_EQ(loiter_alt, current_alt_amsl); -} - -TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) -{ - GeofenceBreachAvoidance gf_avoidance(nullptr); - const float climbrate = 10.0f; - const float current_alt_amsl = 100.0f; - geofence_violation_type_u gf_violation{}; - gf_violation.flags.max_altitude_exceeded = true; - - gf_avoidance.setClimbRate(climbrate); - gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); - gf_avoidance.computeVerticalBrakingDistanceMultirotor(); - - float loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation); - - EXPECT_EQ(loiter_alt_amsl, current_alt_amsl + gf_avoidance.computeVerticalBrakingDistanceMultirotor() - - gf_avoidance.getMinVertDistToFenceMultirotor()); - - gf_violation.flags.max_altitude_exceeded = false; - - loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation); - - EXPECT_EQ(loiter_alt_amsl, current_alt_amsl); -} - -TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter) -{ - GeofenceBreachAvoidance gf_avoidance(nullptr); - FakeGeofence geo; - Vector2d home_global(42.1, 8.2); - MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation{}; - gf_violation.flags.dist_to_home_exceeded = true; - - const float hor_vel = 8.0f; - const float test_point_distance = 30.0f; - const float test_point_bearing = 0.0f; - const float max_dist_to_home = 100.0f; - - gf_avoidance.setHorizontalVelocity(hor_vel); - gf_avoidance.computeBrakingDistanceMultirotor(); - gf_avoidance.setHorizontalTestPointDistance(test_point_distance); - gf_avoidance.setTestPointBearing(test_point_bearing); - - Vector2d current_pos = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, 90.0f); - gf_avoidance.setCurrentPosition(current_pos(0), current_pos(1), 0); - gf_avoidance.setMaxHorDistHome(max_dist_to_home); - gf_avoidance.setHomePosition(home_global(0), home_global(1), 0); - - Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); - - Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, - max_dist_to_home - gf_avoidance.getMinHorDistToFenceMulticopter()); - - EXPECT_LT(Vector2d(loiter_point_predicted - loiter_point_lat_lon).norm(), 1e-4); -} - -TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing) -{ - GeofenceBreachAvoidance gf_avoidance(nullptr); - FakeGeofence geo; - Vector2d home_global(42.1, 8.2); - MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation{}; - gf_violation.flags.dist_to_home_exceeded = true; - - const float test_point_distance = 30.0f; - const float max_dist_to_home = 100.0f; - const float test_point_bearing = 0.0f; - - gf_avoidance.setHorizontalTestPointDistance(test_point_distance); - gf_avoidance.setTestPointBearing(test_point_bearing); - - Vector2d current_pos = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, 90.0f); - gf_avoidance.setCurrentPosition(current_pos(0), current_pos(1), 0); - gf_avoidance.setMaxHorDistHome(max_dist_to_home); - gf_avoidance.setHomePosition(home_global(0), home_global(1), 0); - - Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); - - Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, - max_dist_to_home - 2.0f * test_point_distance); - - EXPECT_LT(Vector2d(loiter_point_predicted - loiter_point_lat_lon).norm(), 1e-4); -} diff --git a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp deleted file mode 100644 index 126c51224ac..00000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp +++ /dev/null @@ -1,136 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file fake_geofence.hpp - * Provides a fake geofence interface to use for testing - * - * @author Roman Bapst - * @author Julian Kent - */ -#pragma once - -#include"../geofence.h" -#include - -class FakeGeofence : public Geofence -{ -public: - FakeGeofence() : - Geofence(nullptr) - {}; - - virtual ~FakeGeofence() {}; - - bool isInsidePolygonOrCircle(double lat, double lon, float altitude) override - { - switch (_probe_function_behavior) { - case ProbeFunction::ALL_POINTS_OUTSIDE: { - return _allPointsOutside(lat, lon, altitude); - } - - case ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE: { - return _left_inside_right_outside(lat, lon, altitude); - } - - case ProbeFunction::RIGHT_INSIDE_LEFT_OUTSIDE: { - return _right_inside_left_outside(lat, lon, altitude); - } - - case ProbeFunction::GF_BOUNDARY_20M_AHEAD: { - return _gf_boundary_is_20m_north(lat, lon, altitude); - } - - default: - return _allPointsOutside(lat, lon, altitude); - } - } - - enum class ProbeFunction { - ALL_POINTS_OUTSIDE = 0, - LEFT_INSIDE_RIGHT_OUTSIDE, - RIGHT_INSIDE_LEFT_OUTSIDE, - GF_BOUNDARY_20M_AHEAD - }; - - void setProbeFunctionBehavior(ProbeFunction func) {_probe_function_behavior = func;} - - -private: - - ProbeFunction _probe_function_behavior = ProbeFunction::ALL_POINTS_OUTSIDE; - - bool _flag_on_left = true; - bool _flag_on_right = false; - - bool _allPointsOutside(double lat, double lon, float alt) - { - return false; - } - - bool _left_inside_right_outside(double lat, double lon, float alt) - { - if (_flag_on_left) { - _flag_on_left = false; - return true; - - } else { - return false; - } - } - - bool _right_inside_left_outside(double lat, double lon, float alt) - { - if (_flag_on_right) { - _flag_on_right = false; - return true; - - } else { - _flag_on_right = true; - return false; - } - } - - bool _gf_boundary_is_20m_north(double lat, double lon, float alt) - { - matrix::Vector2 home_global(42.1, 8.2); - - MapProjection projection{home_global(0), home_global(1)}; - matrix::Vector2f waypoint_local = projection.project(lat, lon); - - if (waypoint_local(0) >= 20.0f) { - return false; - } - - return true; - } -}; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp deleted file mode 100644 index b8ee1f24354..00000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp +++ /dev/null @@ -1,293 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "geofence_breach_avoidance.h" -#include -#include - -using Vector2d = matrix::Vector2; - - -GeofenceBreachAvoidance::GeofenceBreachAvoidance(ModuleParams *parent) : - ModuleParams(parent) -{ - _paramHandle.param_mpc_jerk_max = param_find("MPC_JERK_MAX"); - _paramHandle.param_mpc_acc_hor = param_find("MPC_ACC_HOR"); - _paramHandle.param_mpc_acc_hor_max = param_find("MPC_ACC_HOR_MAX"); - _paramHandle.param_mpc_jerk_auto = param_find("MPC_JERK_AUTO"); - _paramHandle.param_mpc_acc_up_max = param_find("MPC_ACC_UP_MAX"); - _paramHandle.param_mpc_acc_down_max = param_find("MPC_ACC_DOWN_MAX"); - - updateParameters(); -} - -void GeofenceBreachAvoidance::updateParameters() -{ - ModuleParams::updateParams(); - param_get(_paramHandle.param_mpc_jerk_max, &_params.param_mpc_jerk_max); - param_get(_paramHandle.param_mpc_acc_hor, &_params.param_mpc_acc_hor); - param_get(_paramHandle.param_mpc_acc_hor_max, &_params.param_mpc_acc_hor_max); - param_get(_paramHandle.param_mpc_jerk_auto, &_params.param_mpc_jerk_auto); - param_get(_paramHandle.param_mpc_acc_up_max, &_params.param_mpc_acc_up_max); - param_get(_paramHandle.param_mpc_acc_down_max, &_params.param_mpc_acc_down_max); - - updateMinHorDistToFenceMultirotor(); - updateMinVertDistToFenceMultirotor(); -} - -void GeofenceBreachAvoidance::setCurrentPosition(double lat, double lon, float alt) -{ - _current_pos_lat_lon = Vector2d(lat, lon); - _current_alt_amsl = alt; -} - -void GeofenceBreachAvoidance::setHomePosition(double lat, double lon, float alt) -{ - _home_lat_lon(0) = lat; - _home_lat_lon(1) = lon; -} - -matrix::Vector2 GeofenceBreachAvoidance::waypointFromBearingAndDistance(matrix::Vector2 - current_pos_lat_lon, float test_point_bearing, float test_point_distance) -{ - // TODO: Remove this once the underlying geo function has been fixed - if (test_point_distance < 0.0f) { - test_point_distance *= -1.0f; - test_point_bearing = matrix::wrap_2pi(test_point_bearing + M_PI_F); - } - - double fence_violation_test_point_lat, fence_violation_test_point_lon; - waypoint_from_heading_and_distance(current_pos_lat_lon(0), current_pos_lat_lon(1), test_point_bearing, - test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon); - - return Vector2d(fence_violation_test_point_lat, fence_violation_test_point_lon); -} - -Vector2d -GeofenceBreachAvoidance::getFenceViolationTestPoint() -{ - return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _test_point_distance); -} - -Vector2d -GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence) -{ - if (violation_type.flags.fence_violation) { - const float bearing_90_left = matrix::wrap_2pi(_test_point_bearing - M_PI_F * 0.5f); - const float bearing_90_right = matrix::wrap_2pi(_test_point_bearing + M_PI_F * 0.5f); - - double loiter_center_lat, loiter_center_lon; - double fence_violation_test_point_lat, fence_violation_test_point_lon; - - waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_left, - _test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon); - - const bool left_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat, - fence_violation_test_point_lon, _current_alt_amsl); - - waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_right, - _test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon); - - const bool right_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat, - fence_violation_test_point_lon, _current_alt_amsl); - - float bearing_to_loiter_point; - - if (right_side_is_inside_fence && !left_side_is_inside_fence) { - bearing_to_loiter_point = bearing_90_right; - - } else if (left_side_is_inside_fence && !right_side_is_inside_fence) { - bearing_to_loiter_point = bearing_90_left; - - } else { - bearing_to_loiter_point = matrix::wrap_2pi(_test_point_bearing + M_PI_F); - } - - waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_to_loiter_point, - _test_point_distance, &loiter_center_lat, &loiter_center_lon); - - return Vector2d(loiter_center_lat, loiter_center_lon); - - } else if (violation_type.flags.dist_to_home_exceeded) { - - return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - 2 * _test_point_distance, 0.0f)); - - } else { - return _current_pos_lat_lon; - } -} - -Vector2d -GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence) -{ - - if (violation_type.flags.fence_violation) { - float current_distance = _test_point_distance * 0.5f; - float current_min = 0.0f; - float current_max = _test_point_distance; - Vector2d test_point; - - // binary search for the distance from the drone to the geofence in the given direction - while (fabsf(current_max - current_min) > 0.5f) { - test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance); - - if (!geofence->isInsidePolygonOrCircle(test_point(0), test_point(1), _current_alt_amsl)) { - current_max = current_distance; - - } else { - current_min = current_distance; - } - - current_distance = (current_max + current_min) * 0.5f; - } - - test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance); - - if (_multirotor_braking_distance > current_distance - _min_hor_dist_to_fence_mc) { - return waypointFromBearingAndDistance(test_point, _test_point_bearing + M_PI_F, _min_hor_dist_to_fence_mc); - - } else { - return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance); - } - - } else if (violation_type.flags.dist_to_home_exceeded) { - - return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - _min_hor_dist_to_fence_mc, 0.0f)); - - } else { - if (_velocity_hor_abs > 0.5f) { - return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance); - - } else { - return _current_pos_lat_lon; - } - } -} - -float GeofenceBreachAvoidance::generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type) -{ - if (violation_type.flags.max_altitude_exceeded) { - return _current_alt_amsl - 2.0f * _vertical_test_point_distance; - - } else { - return _current_alt_amsl; - } -} - -float GeofenceBreachAvoidance::generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type) -{ - if (violation_type.flags.max_altitude_exceeded) { - return _current_alt_amsl + _multirotor_vertical_braking_distance - _min_vert_dist_to_fence_mc; - - } else { - return _current_alt_amsl; - } -} - -float GeofenceBreachAvoidance::computeBrakingDistanceMultirotor() -{ - const float accel_delay_max = math::max(_params.param_mpc_acc_hor, _params.param_mpc_acc_hor_max); - VelocitySmoothing predictor(accel_delay_max, _velocity_hor_abs, 0.f); - predictor.setMaxVel(_velocity_hor_abs); - predictor.setMaxAccel(_params.param_mpc_acc_hor); - predictor.setMaxJerk(_params.param_mpc_jerk_auto); - predictor.updateDurations(0.f); - predictor.updateTraj(predictor.getTotalTime()); - - _multirotor_braking_distance = predictor.getCurrentPosition() + (GEOFENCE_CHECK_INTERVAL_US / 1e6f) * - _velocity_hor_abs; - - return _multirotor_braking_distance; -} - -float GeofenceBreachAvoidance::computeVerticalBrakingDistanceMultirotor() -{ - const float accel_delay_max = math::max(_params.param_mpc_acc_up_max, _params.param_mpc_acc_down_max); - const float vertical_vel_abs = fabsf(_climbrate); - - - VelocitySmoothing predictor(accel_delay_max, vertical_vel_abs, 0.f); - predictor.setMaxVel(vertical_vel_abs); - predictor.setMaxAccel(_climbrate > 0 ? _params.param_mpc_acc_down_max : _params.param_mpc_acc_up_max); - predictor.setMaxJerk(_params.param_mpc_jerk_auto); - predictor.updateDurations(0.f); - predictor.updateTraj(predictor.getTotalTime()); - - _multirotor_vertical_braking_distance = predictor.getCurrentPosition() + (GEOFENCE_CHECK_INTERVAL_US / 1e6f) * - vertical_vel_abs; - - _multirotor_vertical_braking_distance = matrix::sign(_climbrate) * _multirotor_vertical_braking_distance; - - return _multirotor_vertical_braking_distance; -} - -void GeofenceBreachAvoidance::updateMinHorDistToFenceMultirotor() -{ - const float accel_delay_max = math::max(_params.param_mpc_acc_hor, _params.param_mpc_acc_hor_max); - VelocitySmoothing predictor(accel_delay_max, 0.0f, 0.f); - predictor.setMaxVel(0.0f); - predictor.setMaxAccel(_params.param_mpc_acc_hor); - predictor.setMaxJerk(_params.param_mpc_jerk_auto); - predictor.updateDurations(0.f); - predictor.updateTraj(predictor.getTotalTime()); - - _min_hor_dist_to_fence_mc = 2.0f * predictor.getCurrentPosition(); - -} - -void GeofenceBreachAvoidance::updateMinVertDistToFenceMultirotor() -{ - const float accel_delay_max = math::max(_params.param_mpc_acc_up_max, _params.param_mpc_acc_down_max); - - VelocitySmoothing predictor(accel_delay_max, 0, 0.f); - predictor.setMaxVel(0); - predictor.setMaxAccel(_params.param_mpc_acc_down_max); - predictor.setMaxJerk(_params.param_mpc_jerk_auto); - predictor.updateDurations(0.f); - predictor.updateTraj(predictor.getTotalTime()); - - _min_vert_dist_to_fence_mc = 2.0f * predictor.getCurrentPosition(); -} - -Vector2d GeofenceBreachAvoidance::waypointFromHomeToTestPointAtDist(float distance) -{ - Vector2d test_point = getFenceViolationTestPoint(); - float bearing_home_current_pos = get_bearing_to_next_waypoint(_home_lat_lon(0), _home_lat_lon(1), test_point(0), - test_point(1)); - double loiter_center_lat, loiter_center_lon; - - waypoint_from_heading_and_distance(_home_lat_lon(0), _home_lat_lon(1), bearing_home_current_pos, distance, - &loiter_center_lat, &loiter_center_lon); - - return Vector2d(loiter_center_lat, loiter_center_lon); -} diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h deleted file mode 100644 index 86c2088aecc..00000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h +++ /dev/null @@ -1,144 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include "../geofence.h" -#include - - -class Geofence; - -#define GEOFENCE_CHECK_INTERVAL_US 200000 // 0.2s - -union geofence_violation_type_u { - struct { - bool dist_to_home_exceeded: 1; ///< 0 - distance to home exceeded - bool max_altitude_exceeded: 1; ///< 1 - maximum altitude exceeded - bool fence_violation: 1; ///< 2- violation of user defined fence - } flags; - uint8_t value; -}; - -class GeofenceBreachAvoidance : public ModuleParams -{ -public: - GeofenceBreachAvoidance(ModuleParams *parent); - - ~GeofenceBreachAvoidance() = default; - - matrix::Vector2 getFenceViolationTestPoint(); - - matrix::Vector2 waypointFromBearingAndDistance(matrix::Vector2 current_pos_lat_lon, - float test_point_bearing, float test_point_distance); - - matrix::Vector2 - generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence); - - float computeBrakingDistanceMultirotor(); - - float computeVerticalBrakingDistanceMultirotor(); - - matrix::Vector2 generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence); - - float generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type); - - float generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type); - - float getMinHorDistToFenceMulticopter() {return _min_hor_dist_to_fence_mc;} - - float getMinVertDistToFenceMultirotor() {return _min_vert_dist_to_fence_mc;} - - void setTestPointBearing(float test_point_bearing) { _test_point_bearing = test_point_bearing; } - - void setHorizontalTestPointDistance(float test_point_distance) { _test_point_distance = test_point_distance; } - - void setVerticalTestPointDistance(float distance) { _vertical_test_point_distance = distance; } - - void setHorizontalVelocity(float velocity_hor_abs) { _velocity_hor_abs = velocity_hor_abs; } - - void setClimbRate(float climbrate) { _climbrate = climbrate; } - - void setCurrentPosition(double lat, double lon, float alt); - - void setHomePosition(double lat, double lon, float alt); - - void setMaxHorDistHome(float dist) { _max_hor_dist_home = dist; } - - void updateParameters(); - -private: - struct { - param_t param_mpc_jerk_max; - param_t param_mpc_acc_hor; - param_t param_mpc_acc_hor_max; - param_t param_mpc_jerk_auto; - param_t param_mpc_acc_up_max; - param_t param_mpc_acc_down_max; - - } _paramHandle; - - struct { - float param_mpc_jerk_max; - float param_mpc_acc_hor; - float param_mpc_acc_hor_max; - float param_mpc_jerk_auto; - float param_mpc_acc_up_max; - float param_mpc_acc_down_max; - - } _params; - - float _test_point_bearing{0.0f}; - float _test_point_distance{0.0f}; - float _vertical_test_point_distance{0.0f}; - float _velocity_hor_abs{0.0f}; - float _climbrate{0.0f}; - float _current_alt_amsl{0.0f}; - float _min_hor_dist_to_fence_mc{0.0f}; - float _min_vert_dist_to_fence_mc{0.0f}; - - float _multirotor_braking_distance{0.0f}; - float _multirotor_vertical_braking_distance{0.0f}; - - matrix::Vector2 _current_pos_lat_lon{}; - matrix::Vector2 _home_lat_lon {}; - - float _max_hor_dist_home{0.0f}; - - void updateMinHorDistToFenceMultirotor(); - - void updateMinVertDistToFenceMultirotor(); - - matrix::Vector2 waypointFromHomeToTestPointAtDist(float distance); - -}; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 71d0a6bdf7e..2422b900355 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -138,7 +138,6 @@ public: int getGeofenceAction() { return _param_gf_action.get(); } float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); } - bool getPredict() { return _param_gf_predict.get(); } bool isHomeRequired(); @@ -230,7 +229,6 @@ private: (ParamInt) _param_gf_action, (ParamInt) _param_gf_source, (ParamFloat) _param_gf_max_hor_dist, - (ParamFloat) _param_gf_max_ver_dist, - (ParamBool) _param_gf_predict + (ParamFloat) _param_gf_max_ver_dist ) }; diff --git a/src/modules/navigator/geofence_params.yaml b/src/modules/navigator/geofence_params.yaml index dc0c57301b8..8d594b35fe6 100644 --- a/src/modules/navigator/geofence_params.yaml +++ b/src/modules/navigator/geofence_params.yaml @@ -57,14 +57,3 @@ parameters: min: 0 max: 10000 increment: 1 - GF_PREDICT: - description: - short: '[EXPERIMENTAL] Use Pre-emptive geofence triggering' - long: |- - WARNING: This experimental feature may cause flyaways. Use at your own risk. - - Predict the motion of the vehicle and trigger the breach if it is determined that the current trajectory - would result in a breach happening before the vehicle can make evasive maneuvers. - The vehicle is then re-routed to a safe hold position (stop for multirotor, loiter for fixed wing). - type: boolean - default: 0 diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f7b8f323bcd..7bd2447e975 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,8 +56,6 @@ #include "navigation.h" -#include "GeofenceBreachAvoidance/geofence_breach_avoidance.h" - #include #include @@ -367,16 +365,15 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ Geofence _geofence; /**< class that handles the geofence */ - GeofenceBreachAvoidance _gf_breach_avoidance; hrt_abstime _last_geofence_check{0}; + bool _geofence_reposition_sent{false}; /**< true if a reposition triplet has been sent for the current breach */ + hrt_abstime _time_loitering_after_gf_breach{0}; /**< latches breach state while loitering, prevents reposition center walking */ bool _navigator_status_updated{false}; hrt_abstime _last_navigator_status_publication{0}; hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */ - bool _geofence_reposition_sent{false}; /**< flag if reposition command has been sent for current geofence breach*/ - hrt_abstime _time_loitering_after_gf_breach{0}; /**< timestamp of when loitering after a geofence breach was started */ bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d4dfe91e3c9..04a38fe66ae 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -71,7 +71,6 @@ Navigator::Navigator() : ModuleParams(nullptr), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence(this), - _gf_breach_avoidance(this), _mission(this), _loiter(this), _takeoff(this), @@ -421,7 +420,7 @@ void Navigator::run() rep->next.valid = false; - _time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases + _time_loitering_after_gf_breach = 0; // a manual reposition unlatches the post-breach loiter state } else { mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence\t"); @@ -518,7 +517,7 @@ void Navigator::run() rep->next.valid = false; - _time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases + _time_loitering_after_gf_breach = 0; // a manual reposition unlatches the post-breach loiter state } else { mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t"); @@ -593,7 +592,7 @@ void Navigator::run() rep->current.valid = true; rep->current.timestamp = hrt_absolute_time(); - _time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases + _time_loitering_after_gf_breach = 0; // a manual reposition unlatches the post-breach loiter state } else { mavlink_log_critical(&_mavlink_log_pub, "Orbit is outside geofence"); @@ -645,7 +644,7 @@ void Navigator::run() rep->current.valid = true; rep->current.timestamp = hrt_absolute_time(); - _time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases + _time_loitering_after_gf_breach = 0; // a manual reposition unlatches the post-breach loiter state } else { mavlink_log_critical(&_mavlink_log_pub, "Figure 8 is outside geofence"); @@ -1001,7 +1000,9 @@ void Navigator::run() void Navigator::geofence_breach_check() { - // reset the _time_loitering_after_gf_breach time if no longer in LOITER (and 100ms after it was triggered) + static constexpr hrt_abstime GEOFENCE_CHECK_INTERVAL_US = 200_ms; + + // reset the post-breach loiter latch if we left AUTO_LOITER (and 100ms after it was triggered) if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER && hrt_elapsed_time(&_time_loitering_after_gf_breach) > 100_ms) { _time_loitering_after_gf_breach = 0; @@ -1010,33 +1011,6 @@ void Navigator::geofence_breach_check() if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) { - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - geofence_violation_type_u gf_violation_type{}; - float test_point_bearing; - float test_point_distance; - float vertical_test_point_distance; - - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); - const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); - _gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs); - _gf_breach_avoidance.setClimbRate(-_local_pos.vz); - test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor(); - vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor(); - - } else { - test_point_distance = 2.0f * get_default_loiter_rad(); - vertical_test_point_distance = 5.0f; - - if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) { - test_point_bearing = pos_ctrl_status.nav_bearing; - - } else { - test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); - } - } - double current_latitude = _global_pos.lat; double current_longitude = _global_pos.lon; float current_altitude = _global_pos.alt; @@ -1054,41 +1028,21 @@ void Navigator::geofence_breach_check() return; } - _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(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()) { - 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; - } - if (_time_loitering_after_gf_breach > 0) { - // if we are in the loitering state after breaching a GF, only allow new ones to be set, but not unset - _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); + // while loitering after a breach, only allow new triggers to be set, never cleared. + // Prevents the reposition center from walking outward when the vehicle hovers at the fence boundary. + _geofence_result.geofence_max_dist_triggered |= !_geofence.isCloserThanMaxDistToHome(current_latitude, + current_longitude, current_altitude); + _geofence_result.geofence_max_alt_triggered |= !_geofence.isBelowMaxAltitude(current_altitude); + _geofence_result.geofence_custom_fence_triggered |= !_geofence.isInsidePolygonOrCircle(current_latitude, + current_longitude, current_altitude); } else { - _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); + _geofence_result.geofence_max_dist_triggered = !_geofence.isCloserThanMaxDistToHome(current_latitude, + current_longitude, current_altitude); + _geofence_result.geofence_max_alt_triggered = !_geofence.isBelowMaxAltitude(current_altitude); + _geofence_result.geofence_custom_fence_triggered = !_geofence.isInsidePolygonOrCircle(current_latitude, + current_longitude, current_altitude); } _last_geofence_check = hrt_absolute_time(); @@ -1096,50 +1050,21 @@ void Navigator::geofence_breach_check() _geofence_result.timestamp = hrt_absolute_time(); _geofence_result.geofence_action = _geofence.getGeofenceAction(); - if (_geofence_result.geofence_max_dist_triggered || _geofence_result.geofence_max_alt_triggered || - _geofence_result.geofence_custom_fence_triggered) { + const bool breach = _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 (breach) { if (!_geofence_reposition_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED && _geofence.getGeofenceAction() == geofence_result_s::GF_ACTION_LOITER) { - // we have predicted a geofence violation and if the action is to loiter then - // demand a reposition to a location which is inside the geofence - + // loiter at the current position; we no longer predict ahead of the vehicle position_setpoint_triplet_s *rep = get_reposition_triplet(); - - matrix::Vector2 loiter_center_lat_lon; - - float loiter_altitude_amsl = current_altitude; - double loiter_latitude = current_latitude; - double loiter_longitude = current_longitude; - - if (_geofence.getPredict()) { - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - // the computation of the braking distance does not match the actual braking distance. Until we have a better model - // we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence - loiter_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForMultirotor(gf_violation_type, - &_geofence); - loiter_latitude = loiter_center_lat_lon(0); - loiter_longitude = loiter_center_lat_lon(1); - - loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForMulticopter(gf_violation_type); - - } else { - - loiter_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForFixedWing(gf_violation_type, &_geofence); - loiter_latitude = loiter_center_lat_lon(0); - loiter_longitude = loiter_center_lat_lon(1); - - loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForFixedWing(gf_violation_type); - } - } - rep->current.timestamp = hrt_absolute_time(); rep->current.yaw = NAN; - rep->current.lat = loiter_latitude; - rep->current.lon = loiter_longitude; - rep->current.alt = loiter_altitude_amsl; + rep->current.lat = current_latitude; + rep->current.lon = current_longitude; + rep->current.alt = current_altitude; rep->current.valid = true; rep->current.loiter_radius = get_default_loiter_rad(); rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; @@ -1152,7 +1077,6 @@ void Navigator::geofence_breach_check() } } else { - _geofence_reposition_sent = false; }