UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(navigator): Removed geofence breach avoidance feature (#27465)

Browse Source 
* 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 <bapstroman@gmail.com>

* fix(docs): removed reference to GF_PREDICT

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* 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 <bapstroman@gmail.com>
Co-authored-by: Balduin <balduin@auterion.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
This commit is contained in:
Roman Bapst
2026-06-05 10:47:35 +03:00
committed by GitHub
parent 63ffa3f9dd
commit ece74e8d24
11 changed files with 43 additions and 1038 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
docs/en/config/safety.md
View File

@@ -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 |
| ------------------------------------------------------------------ | ---------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------- |
| <a id="GF_SOURCE"></a>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. |
| <a id="GF_PREDICT"></a>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). |
| <a id="CBRK_FLIGHTTERM"></a>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 |
| ------------------------------------------------------------------ | ---------------------------------------------------------------------------- | --------------------------------------------------------------------------------------- |
| <a id="GF_SOURCE"></a>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. |
| <a id="CBRK_FLIGHTTERM"></a>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

2
src/modules/navigator/CMakeLists.txt
View File

@@ -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

39
src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt
View File

@@ -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)

297
src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
View File

@@ -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 <gtest/gtest.h>
#include "geofence_breach_avoidance.h"
#include "fake_geofence.hpp"
#include <parameters/param.h>
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);
}

136
src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp
View File

@@ -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 <lib/mathlib/mathlib.h>
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<double> 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;
}
};

293
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp
View File

@@ -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 <lib/geo/geo.h>
#include <motion_planning/VelocitySmoothing.hpp>
using Vector2d = matrix::Vector2<double>;
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<double> GeofenceBreachAvoidance::waypointFromBearingAndDistance(matrix::Vector2<double>
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);
}

144
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h
View File

@@ -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 <lib/mathlib/mathlib.h>
#include <px4_platform_common/module_params.h>
#include "../geofence.h"
#include <px4_platform_common/defines.h>
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<double> getFenceViolationTestPoint();
matrix::Vector2<double> waypointFromBearingAndDistance(matrix::Vector2<double> current_pos_lat_lon,
float test_point_bearing, float test_point_distance);
matrix::Vector2<double>
generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence);
float computeBrakingDistanceMultirotor();
float computeVerticalBrakingDistanceMultirotor();
matrix::Vector2<double> 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<double> _current_pos_lat_lon{};
matrix::Vector2<double> _home_lat_lon {};
float _max_hor_dist_home{0.0f};
void updateMinHorDistToFenceMultirotor();
void updateMinVertDistToFenceMultirotor();
matrix::Vector2<double> waypointFromHomeToTestPointAtDist(float distance);
};

4
src/modules/navigator/geofence.h
View File

@@ -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<px4::params::GF_ACTION>) _param_gf_action,
(ParamInt<px4::params::GF_SOURCE>) _param_gf_source,
(ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_gf_max_hor_dist,
(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist,
(ParamBool<px4::params::GF_PREDICT>) _param_gf_predict
(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist
)
};

11
src/modules/navigator/geofence_params.yaml
View File

@@ -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

7
src/modules/navigator/navigator.h
View File

@@ -56,8 +56,6 @@
#include "navigation.h"
#include "GeofenceBreachAvoidance/geofence_breach_avoidance.h"
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/telemetry_status.h>
@@ -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 */

130
src/modules/navigator/navigator_main.cpp
View File

@@ -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::Vector2<double>fence_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<double> 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;
}