From e536868104dcc44ededf69170b578180732e40cf Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 9 Jul 2020 18:44:31 +0200 Subject: [PATCH] Add GeofenceBreachAvoidance class Signed-off-by: Julian Kent --- cmake/gtest/px4_add_gtest.cmake | 3 +- src/modules/navigator/CMakeLists.txt | 6 +- .../GeofenceBreachAvoidance/CMakeLists.txt | 39 +++ .../GeofenceBreachAvoidanceTest.cpp | 233 ++++++++++++++ .../GeofenceBreachAvoidance/fake_geofence.hpp | 183 +++++++++++ .../geofence_breach_avoidance.cpp | 293 ++++++++++++++++++ .../geofence_breach_avoidance.h | 134 ++++++++ src/modules/navigator/geofence.cpp | 61 ++-- src/modules/navigator/geofence.h | 26 +- 9 files changed, 949 insertions(+), 29 deletions(-) create mode 100644 src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt create mode 100644 src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp create mode 100644 src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp create mode 100644 src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp create mode 100644 src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index c3d3fef6da..77b2b5c24f 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -90,10 +90,9 @@ function(px4_add_functional_gtest) # link the libary to test and gtest target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_functional_main - px4_daemon + px4_layer px4_platform modules__uORB - px4_layer systemlib cdev px4_work_queue diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 8298bba7dd..08accf2e23 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -31,6 +31,8 @@ # ############################################################################ +add_subdirectory(GeofenceBreachAvoidance) + px4_add_module( MODULE modules__navigator MAIN navigator @@ -53,4 +55,6 @@ px4_add_module( git_ecl ecl_geo landing_slope - ) + geofence_breach_avoidance + motion_planning + ) \ No newline at end of file diff --git a/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt b/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt new file mode 100644 index 0000000000..0696dbca54 --- /dev/null +++ b/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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 ecl_geo motion_planning) diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp new file mode 100644 index 0000000000..8d94405f7d --- /dev/null +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * 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" + +using namespace matrix; +using Vector2d = matrix::Vector2; + +TEST(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance) +{ + + GeofenceBreachAvoidance gf_avoidance; + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + Vector2d 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); + + float x, y; + map_projection_project(&ref, waypoint_north_east_global(0), waypoint_north_east_global(1), &x, &y); + Vector2d waypoint_north_east_reprojected(x, y); + + 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)); + + + Vector2d waypoint_south_west_local = -waypoint_north_east_local; + + Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5); + + map_projection_project(&ref, waypoint_southwest_global(0), waypoint_southwest_global(1), &x, &y); + Vector2d waypoint_south_west_reprojected(x, y); + + 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_EQ(home_global, same_as_home_global); +} + +TEST(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing) +{ + GeofenceBreachAvoidance gf_avoidance; + FakeGeofence geo; + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + geofence_violation_type_u gf_violation; + gf_violation.flags.fence_violation = true; + + gf_avoidance.setTestPointDistance(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::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::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(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) +{ + GeofenceBreachAvoidance gf_avoidance; + FakeGeofence geo; + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + 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.setTestPointDistance(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::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.getMinDistToFenceMulticopter()); + + 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_EQ(loiter_point, home_global); +} + +TEST(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) +{ + GeofenceBreachAvoidance gf_avoidance; + 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(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) +{ + GeofenceBreachAvoidance gf_avoidance; + 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); +} diff --git a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp new file mode 100644 index 0000000000..d49a883227 --- /dev/null +++ b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp @@ -0,0 +1,183 @@ + + + +#include"../geofence.h" +#include + +class FakeGeofence : public Geofence +{ +public: + FakeGeofence() : + Geofence(nullptr) + {}; + + virtual ~FakeGeofence() {}; + + virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude) + { + switch (_probe_function_behavior) { + case ALL_POINTS_OUTSIDE: { + return _allPointsOutside(lat, lon, altitude); + } + + case LEFT_INSIDE_RIGHT_OUTSIDE: { + return _left_inside_right_outside(lat, lon, altitude); + } + + case RIGHT_INSIDE_LEFT_OUTSIDE: { + return _right_inside_left_outside(lat, lon, altitude); + } + + case GF_BOUNDARY_20M_AHEAD: { + return _gf_boundary_is_20m_ahead(lat, lon, altitude); + } + + default: + return _allPointsOutside(lat, lon, altitude); + } + } + + enum PROBE_FUNC_ENUM { + ALL_POINTS_OUTSIDE = 0, + LEFT_INSIDE_RIGHT_OUTSIDE, + RIGHT_INSIDE_LEFT_OUTSIDE, + GF_BOUNDARY_20M_AHEAD + }; + + void setProbeFunctionBehavior(PROBE_FUNC_ENUM func) {_probe_function_behavior = func;} + + +private: + + PROBE_FUNC_ENUM _probe_function_behavior = ALL_POINTS_OUTSIDE; + + bool _allPointsOutside(double lat, double lon, float alt) + { + return false; + } + + bool _left_inside_right_outside(double lat, double lon, float alt) + { + static int flag = true; + + if (flag) { + flag = false; + return true; + + } else { + return false; + } + } + + bool _right_inside_left_outside(double lat, double lon, float alt) + { + static int flag = false; + + if (flag) { + flag = false; + return true; + + } else { + flag = true; + return false; + } + } + + bool _gf_boundary_is_20m_ahead(double lat, double lon, float alt) + { + struct map_projection_reference_s ref = {}; + matrix::Vector2 home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + float x, y; + map_projection_project(&ref, lat, lon, &x, &y); + matrix::Vector2f waypoint_local(x, y); + + if (waypoint_local(0) >= 20.0f) { + return false; + } + + return true; + } +}; + +typedef enum { + DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ + DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ + DM_PERSIST_VOLATILE /* Data does not survive resets */ +} dm_persitence_t; + +typedef enum { + DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ + DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ + DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alternate between 0 and 1) */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_COMPAT, + DM_KEY_NUM_KEYS /* Total number of item types defined */ +} dm_item_t; + +typedef enum { + DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ + DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ + DM_INIT_REASON_VOLATILE /* Data does not survive reset */ +} dm_reset_reason; + +extern "C" { + __EXPORT ssize_t + dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ) {return 0;}; + + /** write to the data manager store */ + __EXPORT ssize_t + dm_write( + dm_item_t item, /* The item type to store */ + unsigned index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ) {return 0;}; + + /** + * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated + * atomically). + * Note that this lock is independent from dm_read & dm_write calls. + * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) + */ + __EXPORT int + dm_lock( + dm_item_t item /* The item type to lock */ + ) {return 0;}; + + /** + * Try to lock all items of a type (@see sem_trywait()). + * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) + */ + __EXPORT int + dm_trylock( + dm_item_t item /* The item type to lock */ + ) {return 0;}; + + /** Unlock all items of a type */ + __EXPORT void + dm_unlock( + dm_item_t item /* The item type to unlock */ + ) {}; + + /** Erase all items of this type */ + __EXPORT int + dm_clear( + dm_item_t item /* The item type to clear */ + ) {return 0;}; + + /** Tell the data manager about the type of the last reset */ + __EXPORT int + dm_restart( + dm_reset_reason restart_type /* The last reset type */ + ) {return 0;}; +} \ No newline at end of file diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp new file mode 100644 index 0000000000..0ab7606f4a --- /dev/null +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * 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 "geofence_breach_avoidance.h" +#include +#include + +using Vector2d = matrix::Vector2; + + +GeofenceBreachAvoidance::GeofenceBreachAvoidance() : + ModuleParams(nullptr) +{ + _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(); +} + +GeofenceBreachAvoidance::~GeofenceBreachAvoidance() +{ + +} + +void GeofenceBreachAvoidance::updateParameters() +{ + 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(); +} + +void GeofenceBreachAvoidance::setTestPointBearing(float test_point_bearing) +{ + _test_point_bearing = test_point_bearing; +} + +void GeofenceBreachAvoidance::setTestPointDistance(float test_point_distance) +{ + _test_point_distance = test_point_distance; +} + +void GeofenceBreachAvoidance::setVerticalTestPointDistance(float distance) +{ + _vertical_test_point_distance = distance; +} + +void GeofenceBreachAvoidance::setHorizontalVelocity(float velocity_hor_abs) +{ + _velocity_hor_abs = velocity_hor_abs; +} + +void GeofenceBreachAvoidance::setClimbRate(float climb_rate) +{ + _climbrate = climb_rate; +} + +void GeofenceBreachAvoidance::setCurrentPosition(double lat, double lon, float alt) +{ + _current_pos_lat_lon = Vector2d(lat, lon); + _current_alt_amsl = alt; +} + +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 { + 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; + + // logarithmic search for the distance from the drone to the geofence in the given direction + while (abs(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; + current_distance = current_min + (current_max - current_min) * 0.5f; + + } else { + current_min = current_distance; + current_distance = current_min + (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 (_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() + 0.2f * _velocity_hor_abs; // navigator runs at 5Hz + + 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() + 0.2f * + vertical_vel_abs; // navigator runs at 5Hz + + _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(); +} diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h new file mode 100644 index 0000000000..440911b0a1 --- /dev/null +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 +#include "../geofence.h" +#include + + +class Geofence; + +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(); + + ~GeofenceBreachAvoidance(); + + matrix::Vector2 waypointFromBearingAndDistance(matrix::Vector2 current_pos_lat_lon, + float test_point_bearing, float test_point_distance); + + matrix::Vector2 getFenceViolationTestPoint(); + + 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 getMinDistToFenceMulticopter() {return _min_hor_dist_to_fence_mc;} + + float getMinVertDistToFenceMultirotor() {return _min_vert_dist_to_fence_mc;} + + void setTestPointBearing(float test_point_bearing); + + void setTestPointDistance(float test_point_distance); + + void setVerticalTestPointDistance(float distance); + + void setHorizontalVelocity(float velocity_hor_abs); + + void setClimbRate(float climbrate); + + void setCurrentPosition(double lat, double lon, float alt); + + 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{}; + + void updateMinHorDistToFenceMultirotor(); + + void updateMinVertDistToFenceMultirotor(); + +}; \ No newline at end of file diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index e07965cedf..6f4d608cb7 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,7 +58,9 @@ Geofence::Geofence(Navigator *navigator) : _sub_airdata(ORB_ID(vehicle_air_data)) { // we assume there's no concurrent fence update on startup - _updateFence(); + if (_navigator != nullptr) { + _updateFence(); + } } Geofence::~Geofence() @@ -218,14 +220,13 @@ bool Geofence::check(const struct mission_item_s &mission_item) return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude); } -bool Geofence::checkAll(double lat, double lon, float altitude) +bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) { bool inside_fence = true; if (isHomeRequired() && _navigator->home_position_valid()) { const float max_horizontal_distance = _param_gf_max_hor_dist.get(); - const float max_vertical_distance = _param_gf_max_ver_dist.get(); const double home_lat = _navigator->get_home_position()->lat; const double home_lon = _navigator->get_home_position()->lon; @@ -236,20 +237,10 @@ bool Geofence::checkAll(double lat, double lon, float altitude) get_distance_to_point_global_wgs84(lat, lon, altitude, home_lat, home_lon, home_alt, &dist_xy, &dist_z); - if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) { - if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home exceeded by %.1f m", - (double)(dist_z - max_vertical_distance)); - _last_vertical_range_warning = hrt_absolute_time(); - } - - inside_fence = false; - } - if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) { if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home exceeded by %.1f m", - (double)(dist_xy - max_horizontal_distance)); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)", + (double)max_horizontal_distance); _last_horizontal_range_warning = hrt_absolute_time(); } @@ -257,9 +248,44 @@ bool Geofence::checkAll(double lat, double lon, float altitude) } } + return inside_fence; +} + +bool Geofence::isBelowMaxAltitude(float altitude) +{ + bool inside_fence = true; + + if (isHomeRequired() && _navigator->home_position_valid()) { + + const float max_vertical_distance = _param_gf_max_ver_dist.get(); + const float home_alt = _navigator->get_home_position()->alt; + + float dist_z = altitude - home_alt; + + if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) { + if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)", + (double)max_vertical_distance); + _last_vertical_range_warning = hrt_absolute_time(); + } + + inside_fence = false; + } + } + + return inside_fence; +} + + +bool Geofence::checkAll(double lat, double lon, float altitude) +{ + bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude); + + inside_fence = inside_fence && isBelowMaxAltitude(altitude); + // to be inside the geofence both fences have to report being inside // as they both report being inside when not enabled - inside_fence = inside_fence && checkPolygons(lat, lon, altitude); + inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude); if (inside_fence) { _outside_counter = 0; @@ -277,8 +303,7 @@ bool Geofence::checkAll(double lat, double lon, float altitude) } } - -bool Geofence::checkPolygons(double lat, double lon, float altitude) +bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) { // the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means // the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 207c1677ca..90d896cea1 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,7 @@ #include +#include #include #include #include @@ -62,7 +63,7 @@ public: Geofence(Navigator *navigator); Geofence(const Geofence &) = delete; Geofence &operator=(const Geofence &) = delete; - ~Geofence(); + virtual ~Geofence(); /* Altitude mode, corresponding to the param GF_ALTMODE */ enum { @@ -97,6 +98,13 @@ public: */ bool check(const struct mission_item_s &mission_item); + + bool isCloserThanMaxDistToHome(double lat, double lon, float altitude); + + bool isBelowMaxAltitude(float altitude); + + virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude); + int clearDm(); bool valid(); @@ -130,6 +138,14 @@ public: bool isHomeRequired(); + /** + * Check if a point passes the Geofence test. + * In addition to checkPolygons(), this takes all additional parameters into account. + * + * @return false for a geofence violation + */ + bool checkAll(double lat, double lon, float altitude); + /** * print Geofence status to the console */ @@ -188,13 +204,7 @@ private: */ bool checkPolygons(double lat, double lon, float altitude); - /** - * Check if a point passes the Geofence test. - * In addition to checkPolygons(), this takes all additional parameters into account. - * - * @return false for a geofence violation - */ - bool checkAll(double lat, double lon, float altitude); + bool checkAll(const vehicle_global_position_s &global_position); bool checkAll(const vehicle_global_position_s &global_position, float baro_altitude_amsl);