mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-26 16:28:06 +08:00
40e61d2429
Plan RTL paths that route around geofence boundaries — both inclusion and exclusion zones — instead of flying straight through them and breaching. The planner builds a visibility graph over the margin-inflated geofence polygons and circles and runs Dijkstra to find the shortest legal return path, falling back to a straight line to the destination when no valid path exists. Highlights: - New reusable libraries: src/lib/dijkstra (generic shortest path) and src/lib/geofence (fixed-point geometry, polygon inflation, bitangent visibility); the RTL planner lives in navigator/RTLPlanner. - Visibility graph keeps only bitangent edges and skips edges that poke into a forbidden region, greatly reducing edge-cost computation. - Corner splitting is limited to sharp convex corners. - Geometry validation: reject self-intersecting polygons and vertices outside the fixed-point range; centimeter fixed-point scaling keeps orientation tests exact and avoids drift at large distances. - Failure handling: a Status enum replaces silent bool returns, and failures (unbuildable fence, planner capacity overflow, dataman load errors, NaN waypoints, destinations that breach the fence) are surfaced to the operator via MAVLink warnings/criticals; the planner falls back to a straight-line RTL. - Destination updates are centralized in RTL::setRtlTypeAndDestination; the path is replanned only when the destination or geometry changes. - VTOLs always use the fixed-wing margin (FW loiter radius). - kMaxNodes is exposed as a Kconfig option (default 100); the feature is disabled on flash-constrained boards (FMUv4 and older, various F4/F7). - Tests: unit tests for the Dijkstra lib, geofence geometry utils, and the avoidance planner, plus a MAVSDK SITL test flying an RTL through a geofence. - Documentation: new "Geofence Awareness" section in the RTL docs. Co-authored-by: Balduin <balduin@auterion.com>
58 lines
1.6 KiB
CMake
58 lines
1.6 KiB
CMake
cmake_minimum_required(VERSION 3.5.1)
|
|
|
|
project(mavsdk_tests CXX)
|
|
|
|
set(CMAKE_CXX_STANDARD 17)
|
|
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
|
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
|
|
|
file(READ MAVSDK_VERSION MAVSDK_VERSION)
|
|
string(STRIP ${MAVSDK_VERSION} MAVSDK_VERSION)
|
|
|
|
find_package(MAVSDK "${MAVSDK_VERSION}" REQUIRED)
|
|
find_package(Threads REQUIRED)
|
|
|
|
if(MAVSDK_FOUND)
|
|
add_executable(mavsdk_tests
|
|
test_main.cpp
|
|
autopilot_tester.cpp
|
|
autopilot_tester_failure.cpp
|
|
autopilot_tester_rtl.cpp
|
|
autopilot_tester_figure_eight.cpp
|
|
# follow-me needs a MAVSDK update:
|
|
# https://github.com/mavlink/MAVSDK/pull/1770
|
|
# autopilot_tester_follow_me.cpp
|
|
test_multicopter_basics.cpp
|
|
test_multicopter_control_allocation.cpp
|
|
test_multicopter_failure_injection.cpp
|
|
test_multicopter_failsafe.cpp
|
|
test_multicopter_mission.cpp
|
|
test_multicopter_offboard.cpp
|
|
test_multicopter_manual.cpp
|
|
test_multicopter_rtl_with_geofence.cpp
|
|
test_vtol_mission.cpp
|
|
test_vtol_figure_eight.cpp
|
|
test_vtol_rtl.cpp
|
|
test_vtol_mission_wind.cpp
|
|
test_vtol_loiter_airspeed_failure_blockage.cpp
|
|
# test_multicopter_follow_me.cpp
|
|
)
|
|
|
|
target_link_libraries(mavsdk_tests
|
|
MAVSDK::mavsdk
|
|
${CMAKE_THREAD_LIBS_INIT}
|
|
)
|
|
|
|
target_include_directories(mavsdk_tests PUBLIC ${CMAKE_BINARY_DIR}/..)
|
|
|
|
target_compile_options(mavsdk_tests
|
|
PRIVATE
|
|
-Wall
|
|
-Wextra
|
|
-Werror
|
|
-Wno-error=deprecated-declarations
|
|
)
|
|
else()
|
|
message("MAVSDK C++ not found, skipping mavsdk_tests build..")
|
|
endif()
|