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>
63 lines
2.5 KiB
C++
63 lines
2.5 KiB
C++
/***************************************************************************
|
|
*
|
|
* Copyright (c) 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.
|
|
*
|
|
****************************************************************************/
|
|
/**
|
|
* @file rtl_base.h
|
|
*
|
|
* Helper class for RTL modes using the mission
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "mission_base.h"
|
|
#include <uORB/topics/rtl_time_estimate.h>
|
|
#include <matrix/math.hpp>
|
|
|
|
class RtlBase : public MissionBase
|
|
{
|
|
public:
|
|
RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed):
|
|
MissionBase(navigator, dataman_cache_size_signed, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {};
|
|
virtual ~RtlBase() = default;
|
|
|
|
virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0;
|
|
|
|
virtual void setReturnAltMin(bool min) { (void)min;};
|
|
|
|
virtual void setRtlAlt(float alt) { (void)alt;};
|
|
|
|
#if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
|
|
virtual matrix::Vector2d getRtlPlannerDestination() { return {(double)NAN, (double)NAN}; }
|
|
#endif // CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE
|
|
};
|