mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
Adds Smart Return To Home capability by recording the flight path in a memory optimimized format. This means the UAV can return to home by using only known-good flight paths.
The flight graph is stored as a series of delta elements at 1m resolution and a list of nodes. Both lists share the same buffer. Each delta element takes up 2 bytes or sometimes 6 bytes if the delta is large. The path can consist of multiple disconnected segments, in which case the gaps are stored as delta elements with a jump-bit set. Once in a while or when required the graph is consolidated, which means: - Points that lie roughly in a line are replaced by a single line - Points that lie close to previously recorded lines are pruned - For lines that pass close to each other a node element is created Furthermore: - The graph is recorded at a higher precision closer to home - If the graph becomes full, the overall precision is reduced and the whole graph is re-consolidated - If the graph becomes full once more, all data is removed except for the shortest path home at that moment. One of these actions is repeated at each subsequent fill-up. Path finding information is generated/refreshed on demand and stored in the nodes. During return-to-home, the best direction to home is continuously evaluated by using the information stored in the nodes. The graph recording and path finding is implemented in navigator/tracker.cpp. The graph based return-to-home is implemented in navigator/smart_rtl.cpp.
This commit is contained in:
Samuel Sadok
committed by
Daniel Agar
Daniel Agar
parent
eabbd19c1c
commit
ee75ebac8c
@@ -0,0 +1,110 @@
|
||||
% This script can be used to create test cases for the line-to-line
|
||||
% distance function in the flight path tracker. We can define deltas and
|
||||
% start points for two lines and get the shortest delta between the two
|
||||
% lines, as well as a visualisation of it. Note that all points are
|
||||
% discretized, so they will usually not lie exactly on the lines.
|
||||
|
||||
function linesToLineTest()
|
||||
% plot different scenarios
|
||||
delta = plotLines([4; 2; -1], [2; 1; -2], [6; 2; 2], [2; 3; 0])
|
||||
delta = plotLines([4; 2; -1], [2; 1; -2], [6; 2; 2], [5; 4; 4])
|
||||
delta = plotLines([4; 2; -1], [2; 2; -2], [4; 1; 1], [5; 4; 4])
|
||||
delta = plotLines([4; 2; -1], [2; 1; -2], [3; 1; 1], [-1; 2; -2])
|
||||
delta = plotLines([4; 2; -1], [2; 1; -2], [-2; 3; 2], [-1; 2; -2])
|
||||
delta = plotLines([4; 2; -1], [2; 1; -2], [-2; 3; 2], [-3; 5; 0])
|
||||
delta = plotLines([4; 2; -1], [6; 3; -3], [-2; 3; 2], [-3; 5; 0])
|
||||
|
||||
delta = plotLines([4; 2; -1], [2; 1; -2], [-2; 3; 2], [-1; 2; -2])
|
||||
end
|
||||
|
||||
% This creates a plot showing the two lines and 5 connections between them:
|
||||
% - the shortest possible connection
|
||||
% - the start of line1 projected to line2
|
||||
% - the end of line1 projected to line2
|
||||
% - the start of line2 projected to line1
|
||||
% - the end of line2 projected to line1
|
||||
% The projected points are constrained such that they don't lie beyond
|
||||
% the start or end of the other line.
|
||||
function delta = plotLines(deltaA, endA, deltaB, endB)
|
||||
COEF1 = 32768;
|
||||
|
||||
startA = endA - coefToFloat(COEF1-1) * deltaA;
|
||||
startB = endB - coefToFloat(COEF1-1) * deltaB;
|
||||
|
||||
projectedStartACoef = coefFromFloat(-dot(startA-endB,deltaB) / dot(deltaB,deltaB));
|
||||
projectedEndACoef = coefFromFloat(-dot(endA-endB,deltaB) / dot(deltaB,deltaB));
|
||||
projectedStartBCoef = coefFromFloat(-dot(startB-endA,deltaA) / dot(deltaA,deltaA));
|
||||
projectedEndBCoef = coefFromFloat(-dot(endB-endA,deltaA) / dot(deltaA,deltaA));
|
||||
|
||||
normal = [deltaA(2)*deltaB(3)-deltaA(3)*deltaB(2);deltaA(3)*deltaB(1)-deltaA(1)*deltaB(3);deltaA(1)*deltaB(2)-deltaA(2)*deltaB(1)];
|
||||
projectedDelta = normal*dot(endB-endA,normal)/dot(normal,normal);
|
||||
|
||||
% Rounding is probably appropriate here, as the precision is not
|
||||
% critical and it allows for a more efficient implementation.
|
||||
% In a few cases the final result will differ, but visual inspection
|
||||
% indicates that this is acceptable.
|
||||
projectedDelta = round(projectedDelta);
|
||||
|
||||
remainder = endB-endA - projectedDelta;
|
||||
|
||||
A=[-deltaA deltaB];
|
||||
lineToLineCoef = (A'*A) \ (A'*remainder);
|
||||
lineToLineCoef(1) = coefFromFloatUnconstrained(lineToLineCoef(1));
|
||||
lineToLineCoef(2) = coefFromFloatUnconstrained(lineToLineCoef(2));
|
||||
|
||||
|
||||
close all;
|
||||
hold on;
|
||||
axis equal;
|
||||
plot3([endA(1) startA(1)], [endA(2) startA(2)], [endA(3) startA(3)], 'o-r');
|
||||
plot3([endB(1) startB(1)], [endB(2) startB(2)], [endB(3) startB(3)], 'x-b');
|
||||
delta = plotLine(deltaA, endA, deltaB, endB, lineToLineCoef(1), lineToLineCoef(2))';
|
||||
delta1 = plotLine(deltaA, endA, deltaB, endB, COEF1, projectedStartACoef)';
|
||||
delta2 = plotLine(deltaA, endA, deltaB, endB, 0, projectedEndACoef)';
|
||||
delta3 = plotLine(deltaA, endA, deltaB, endB, projectedStartBCoef, COEF1)';
|
||||
delta4 = plotLine(deltaA, endA, deltaB, endB, projectedEndBCoef, 0)';
|
||||
legend('line A', 'line B', 'line-to-line', 'start1 to line2', 'end1 to line2', 'start2 to line1', 'end2 to line1');
|
||||
hold off;
|
||||
|
||||
%disp(lineToLineCoef);
|
||||
if (lineToLineCoef(1) >= 0 && lineToLineCoef(1) < COEF1 && lineToLineCoef(2) >= 0 && lineToLineCoef(2) < COEF1)
|
||||
return
|
||||
end
|
||||
|
||||
delta = delta1;
|
||||
if (norm(delta2) < norm(delta))
|
||||
delta = delta2;
|
||||
end
|
||||
if (norm(delta3) < norm(delta))
|
||||
delta = delta3;
|
||||
end
|
||||
if (norm(delta4) < norm(delta))
|
||||
delta = delta4;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
function coef = coefFromFloatUnconstrained(f)
|
||||
coef = round(f * 32768);
|
||||
end
|
||||
|
||||
function coef = coefFromFloat(f)
|
||||
coef = coefFromFloatUnconstrained(f);
|
||||
if coef < 0
|
||||
coef = 0;
|
||||
elseif coef > 32767
|
||||
coef = 32767;
|
||||
end
|
||||
end
|
||||
|
||||
function f = coefToFloat(coef)
|
||||
f = coef / 32768;
|
||||
end
|
||||
|
||||
function delta = plotLine(deltaA, endA, deltaB, endB, coefA, coefB)
|
||||
p1 = round(endA - deltaA * coefToFloat(coefA));
|
||||
p2 = round(endB - deltaB * coefToFloat(coefB));
|
||||
delta = p2 - p1;
|
||||
%disp([sqrt(dot(delta, delta)) (p2 - p1)']); % for debugging
|
||||
plot3([p1(1) p2(1)], [p1(2) p2(2)], [p1(3) p2(3)], '*--');
|
||||
end
|
||||
Executable
+241
@@ -0,0 +1,241 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# Script to draw 2D flight paths. The z-component is always 0.
|
||||
# The purpose is to test the flight graph tracker by handing it a predesigned path.
|
||||
#
|
||||
# Usage: draw_path.sh [file]
|
||||
# If the file already exists, new points are appended.
|
||||
# arrow keys: move cursor
|
||||
# spacebar: add current cursor position to path
|
||||
# q: quit
|
||||
|
||||
|
||||
|
||||
# Shows a status message at the end of the screen
|
||||
function show_status() {
|
||||
tput cup $(expr $SCREEN_WIDTH-1) 0
|
||||
echo $1
|
||||
move_cursor
|
||||
STATUS_CLEAR="no"
|
||||
}
|
||||
|
||||
# Clears the status message
|
||||
function clear_status() {
|
||||
[ "$STATUS_CLEAR" = "no" ] || return 0
|
||||
tput cup 0 0
|
||||
tput el && tput el1
|
||||
move_cursor
|
||||
STATUS_CLEAR="yes"
|
||||
}
|
||||
|
||||
# Terminates the script
|
||||
function quit() {
|
||||
tput rmcup
|
||||
stty echo
|
||||
exit 0
|
||||
}
|
||||
|
||||
# Moves the terminal cursor to position ($CURSOR_X, $CURSOR_Y)
|
||||
function move_cursor() {
|
||||
#echo "\033[$CURSOR_Y;$CURSOR_X"'f'
|
||||
#echo -e "\033[$CURSOR_Y;3f"
|
||||
tput cup $CURSOR_Y $CURSOR_X
|
||||
}
|
||||
|
||||
# Draws a line from the last point to the current cursor position
|
||||
function go_here() {
|
||||
draw_line $LAST_POINT_X $LAST_POINT_Y $CURSOR_X $CURSOR_Y
|
||||
echo "o"
|
||||
move_cursor
|
||||
LAST_POINT_X=$CURSOR_X
|
||||
LAST_POINT_Y=$CURSOR_Y
|
||||
}
|
||||
|
||||
# Adds the current cursor position to the file $FILE
|
||||
function store_point() {
|
||||
echo "$CURSOR_X,$CURSOR_Y,0," >> $FILE
|
||||
}
|
||||
|
||||
# Loads all points from the file $FILE
|
||||
# Each line stores a point in the format "x,y,z".
|
||||
# Empty lines and lines starting with "/" are ignored.
|
||||
function load_file() {
|
||||
while IFS='' read -r line || [[ -n "$line" ]]; do
|
||||
CURSOR_X=$(echo "$line" | awk -F, "/^[^\/]/{print \$1}")
|
||||
CURSOR_Y=$(echo "$line" | awk -F, "/^[^\/]/{print \$2}")
|
||||
#echo "X=$CURSOR_X Y=$CURSOR_Y"
|
||||
[ "$CURSOR_X" = "" ] || [ "$CURSOR_Y" = "" ] || go_here
|
||||
done < "$FILE"
|
||||
}
|
||||
|
||||
# Draws a line using the Bresenham algorithm
|
||||
# usage: draw_line x1 y1 x2 y2
|
||||
# The point (x1, y1) is not filled.
|
||||
# The point (x2, y2) is filled and the cursor is placed there.
|
||||
function draw_line() {
|
||||
X1=$1
|
||||
Y1=$2
|
||||
X2=$3
|
||||
Y2=$4
|
||||
LINE_CHAR="\\"
|
||||
|
||||
# The algorithm requires that delta-y is smaller than delta-x
|
||||
SWAP_XY=$(echo "define delta(a, b) {if (a>b) return a-b; return b-a;}; delta($X1,$X2) < delta($Y1,$Y2)" | bc)
|
||||
[ "$SWAP_XY" = "1" ] && {
|
||||
TEMP=$X1; X1=$Y1; Y1=$TEMP
|
||||
TEMP=$X2; X2=$Y2; Y2=$TEMP
|
||||
}
|
||||
|
||||
# Delta-x must be positive
|
||||
REVERSE_DIR=$(echo "$X1 > $X2" | bc)
|
||||
[ "$REVERSE_DIR" = "1" ] && {
|
||||
TEMP=$X1; X1=$X2; X2=$TEMP
|
||||
TEMP=$Y1; Y1=$Y2; Y2=$TEMP
|
||||
}
|
||||
|
||||
# Now the slope is in [-45°, +45], in positive x-direction. The update update differs for positive and negative slopes.
|
||||
POS_SLOPE=$(echo "$Y2 > $Y1" | bc)
|
||||
|
||||
DELTA_X=$(($X2-$X1))
|
||||
DELTA_Y=$(($Y2-$Y1))
|
||||
|
||||
# init update criterion
|
||||
if [ "$POS_SLOPE" = "1" ]; then
|
||||
D=$(echo "$DELTA_Y * ($X1+1) - $DELTA_X * ($Y1+0.5) + $X2 * $Y1 - $X1 * $Y2" | bc)
|
||||
else
|
||||
D=$(echo "$DELTA_Y * ($X1+1) - $DELTA_X * ($Y1-0.5) + $X2 * $Y1 - $X1 * $Y2" | bc)
|
||||
fi
|
||||
|
||||
XP=$X1
|
||||
YP=$Y1
|
||||
|
||||
while [ $XP -lt $X2 ]; do
|
||||
[ "$SWAP_XY" = "0" ] && LINE_CHAR="-" || LINE_CHAR="|"
|
||||
|
||||
# Move to next pixel, according to update criterion
|
||||
XP=$(($XP+1))
|
||||
if [ "$POS_SLOPE" = "1" ]; then
|
||||
[ $(echo "$D > 0" | bc) = "1" ] && {
|
||||
YP=$(($YP+1))
|
||||
D=$(echo "$D - $DELTA_X" | bc)
|
||||
LINE_CHAR="\\"
|
||||
}
|
||||
D=$(echo "$D + $DELTA_Y" | bc)
|
||||
else
|
||||
[ $(echo "$D < 0" | bc) = "1" ] && {
|
||||
YP=$(($YP-1))
|
||||
D=$(echo "$D + $DELTA_X" | bc)
|
||||
LINE_CHAR="/"
|
||||
}
|
||||
D=$(echo "$D + $DELTA_Y" | bc)
|
||||
fi
|
||||
|
||||
# Draw pixel
|
||||
[ "$SWAP_XY" = "0" ] && { CURSOR_X=$XP; CURSOR_Y=$YP; } || { CURSOR_X=$YP; CURSOR_Y=$XP; }
|
||||
move_cursor;
|
||||
|
||||
([ "$REVERSE_DIR" = "1" ] && [ "$XP" = "$X2" ]) || echo "$LINE_CHAR"
|
||||
done
|
||||
|
||||
[ "$REVERSE_DIR" = "1" ] && { [ "$SWAP_XY" = "0" ] && { CURSOR_X=$X1; CURSOR_Y=$Y1; } || { CURSOR_X=$Y1; CURSOR_Y=$X1; } }
|
||||
move_cursor;
|
||||
}
|
||||
|
||||
# Moves the cursor up if possible
|
||||
function up() {
|
||||
[ $CURSOR_Y -gt 0 ] && let CURSOR_Y=$CURSOR_Y-1
|
||||
move_cursor
|
||||
}
|
||||
|
||||
# Moves the cursor down if possible
|
||||
function down() {
|
||||
let CURSOR_Y=$CURSOR_Y+1
|
||||
[ $CURSOR_Y -lt $SCREEN_HEIGHT ] || let CURSOR_Y=$SCREEN_HEIGHT-1
|
||||
move_cursor
|
||||
}
|
||||
|
||||
# Moves the cursor left if possible
|
||||
function left() {
|
||||
[ $CURSOR_X -gt 0 ] && let CURSOR_X=$CURSOR_X-1
|
||||
move_cursor
|
||||
}
|
||||
|
||||
# Moves the cursor right if possible
|
||||
function right() {
|
||||
let CURSOR_X=$CURSOR_X+1
|
||||
[ $CURSOR_X -lt $SCREEN_WIDTH ] || let CURSOR_X=$SCREEN_WIDTH-1
|
||||
move_cursor
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
# Set up globals
|
||||
SCREEN_WIDTH=$(tput cols)
|
||||
SCREEN_HEIGHT=$(tput lines)
|
||||
let CURSOR_X=$SCREEN_WIDTH/2
|
||||
let CURSOR_Y=$SCREEN_HEIGHT/2
|
||||
LAST_POINT_X=$CURSOR_X
|
||||
LAST_POINT_Y=$CURSOR_Y
|
||||
KEY_SEQUENCE=""
|
||||
FILE=$1
|
||||
|
||||
touch $FILE 2>/dev/null || { echo "could not open file $FILE"; exit 1; }
|
||||
|
||||
# Switch to alternate screen
|
||||
tput smcup
|
||||
#x=$(tput lines)
|
||||
#while [ $x -gt 0 ]; do echo ""; let x=$x-1; done;
|
||||
tput clear
|
||||
stty -echo
|
||||
move_cursor
|
||||
|
||||
load_file
|
||||
|
||||
|
||||
# draws two crosses
|
||||
#draw_line 20 3 50 15
|
||||
#draw_line 20 15 50 3
|
||||
#draw_line 5 5 20 30
|
||||
#draw_line 20 5 5 30
|
||||
|
||||
# draws the same two crosses (with swapped start end end points)
|
||||
#draw_line 50 15 20 3
|
||||
#draw_line 50 3 20 15
|
||||
#draw_line 20 30 5 5
|
||||
#draw_line 5 30 20 5
|
||||
|
||||
|
||||
while true; do
|
||||
IFS=""
|
||||
read -r -n 1 CHAR
|
||||
|
||||
KEY_SEQUENCE=$KEY_SEQUENCE$CHAR
|
||||
CARRY_SEQUENCE=""
|
||||
|
||||
clear_status
|
||||
|
||||
case $KEY_SEQUENCE in
|
||||
$'\033'|$'\033[')
|
||||
# incomplete escape sequence - read another char
|
||||
CARRY_SEQUENCE=$KEY_SEQUENCE
|
||||
#show_status "press q to exit"
|
||||
;;
|
||||
|
||||
'q') quit ;;
|
||||
|
||||
$'\033[A') up ;;
|
||||
$'\033[B') down ;;
|
||||
$'\033[C') right ;;
|
||||
$'\033[D') left ;;
|
||||
|
||||
$" ") go_here; store_point ;;
|
||||
|
||||
*)
|
||||
show_status "unknown key"
|
||||
;;
|
||||
esac
|
||||
|
||||
KEY_SEQUENCE=$CARRY_SEQUENCE
|
||||
done
|
||||
|
||||
@@ -27,6 +27,7 @@ set(tests
|
||||
microbench_matrix
|
||||
microbench_uorb
|
||||
mixer
|
||||
navigator
|
||||
param
|
||||
parameters
|
||||
perf
|
||||
|
||||
@@ -57,6 +57,8 @@ __BEGIN_DECLS
|
||||
*/
|
||||
typedef uint64_t hrt_abstime;
|
||||
|
||||
#define HRT_ABSTIME_MAX UINT64_MAX
|
||||
|
||||
/**
|
||||
* Callout function type.
|
||||
*
|
||||
|
||||
@@ -34,6 +34,8 @@
|
||||
px4_add_module(
|
||||
MODULE modules__navigator
|
||||
MAIN navigator
|
||||
COMPILE_FLAGS
|
||||
-Wno-error=cast-align
|
||||
SRCS
|
||||
navigator_main.cpp
|
||||
navigator_mode.cpp
|
||||
@@ -49,8 +51,14 @@ px4_add_module(
|
||||
enginefailure.cpp
|
||||
gpsfailure.cpp
|
||||
follow_target.cpp
|
||||
smart_rtl.cpp
|
||||
tracker.cpp
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_geo
|
||||
landing_slope
|
||||
)
|
||||
|
||||
if(PX4_TESTING)
|
||||
add_subdirectory(navigator_tests)
|
||||
endif()
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-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
|
||||
@@ -51,7 +51,9 @@
|
||||
#include "mission.h"
|
||||
#include "navigator_mode.h"
|
||||
#include "rtl.h"
|
||||
#include "smart_rtl.h"
|
||||
#include "takeoff.h"
|
||||
#include "tracker.h"
|
||||
|
||||
#include "navigation.h"
|
||||
|
||||
@@ -81,7 +83,7 @@
|
||||
/**
|
||||
* Number of navigation modes that need on_active/on_inactive calls
|
||||
*/
|
||||
#define NAVIGATOR_MODE_ARRAY_SIZE 9
|
||||
#define NAVIGATOR_MODE_ARRAY_SIZE 10
|
||||
|
||||
|
||||
class Navigator : public ModuleBase<Navigator>, public ModuleParams
|
||||
@@ -169,6 +171,11 @@ public:
|
||||
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
|
||||
float get_loiter_radius() { return _param_nav_loiter_rad.get(); }
|
||||
|
||||
Tracker *get_tracker() { return &_tracker; }
|
||||
|
||||
// Quick and dirty, to do it cleanly, we may want to introduce a new navigator mode
|
||||
void set_rtl_variant(bool advanced) { _use_advanced_rtl = advanced; }
|
||||
|
||||
/**
|
||||
* Returns the default acceptance radius defined by the parameter
|
||||
*/
|
||||
@@ -285,6 +292,12 @@ public:
|
||||
|
||||
bool abort_landing();
|
||||
|
||||
// Advanced RTL
|
||||
void tracker_reset() { _tracker.reset_graph(); }
|
||||
void tracker_consolidate() { _tracker.consolidate_graph(); }
|
||||
void tracker_rewrite() { _tracker.rewrite_graph(); }
|
||||
|
||||
|
||||
// Param access
|
||||
float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
|
||||
float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
|
||||
@@ -368,21 +381,25 @@ private:
|
||||
Geofence _geofence; /**< class that handles the geofence */
|
||||
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
|
||||
|
||||
Tracker _tracker; /**< class that tracks the vehicle path for smart RTL **/
|
||||
|
||||
bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */
|
||||
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 */
|
||||
bool _use_advanced_rtl{true}; /**< use graph-based RTL instead of direct RTL **/
|
||||
|
||||
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
|
||||
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
|
||||
Mission _mission; /**< class that handles the missions */
|
||||
Loiter _loiter; /**< class that handles loiter */
|
||||
Takeoff _takeoff; /**< class for handling takeoff commands */
|
||||
Land _land; /**< class for handling land commands */
|
||||
Land _land; /**< class for handling land commands */
|
||||
PrecLand _precland; /**< class for handling precision land commands */
|
||||
RTL _rtl; /**< class that handles RTL */
|
||||
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
|
||||
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
|
||||
FollowTarget _follow_target;
|
||||
SmartRTL _smartRtl; /**< class that handles return-to-land along recorded flight graph */
|
||||
|
||||
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
|
||||
|
||||
|
||||
@@ -86,7 +86,8 @@ Navigator::Navigator() :
|
||||
_rtl(this),
|
||||
_engineFailure(this),
|
||||
_gpsFailure(this),
|
||||
_follow_target(this)
|
||||
_follow_target(this),
|
||||
_smartRtl(this)
|
||||
{
|
||||
/* Create a list of our possible navigation types */
|
||||
_navigation_mode_array[0] = &_mission;
|
||||
@@ -98,6 +99,7 @@ Navigator::Navigator() :
|
||||
_navigation_mode_array[6] = &_land;
|
||||
_navigation_mode_array[7] = &_precland;
|
||||
_navigation_mode_array[8] = &_follow_target;
|
||||
_navigation_mode_array[9] = &_smartRtl;
|
||||
|
||||
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
|
||||
_handle_reverse_delay = param_find("VT_B_REV_DEL");
|
||||
@@ -176,7 +178,18 @@ Navigator::run()
|
||||
} else {
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/* success, local pos is available */
|
||||
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
|
||||
if (orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos) == PX4_OK) {
|
||||
if (!_land_detected.landed) {
|
||||
if (_tracker.get_graph_fault()) {
|
||||
_tracker.reset_graph();
|
||||
}
|
||||
|
||||
_tracker.update(&_local_pos);
|
||||
|
||||
} else {
|
||||
_use_advanced_rtl = true; // Try advanced RTL again for the next flight
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -212,9 +225,21 @@ Navigator::run()
|
||||
params_update();
|
||||
}
|
||||
|
||||
_land_detected_sub.update(&_land_detected);
|
||||
if (_land_detected_sub.update(&_land_detected)) {
|
||||
if (!_land_detected.landed) {
|
||||
if (_tracker.get_graph_fault()) {
|
||||
_tracker.reset_graph();
|
||||
}
|
||||
|
||||
_tracker.update(&_local_pos);
|
||||
}
|
||||
}
|
||||
|
||||
_position_controller_status_sub.update();
|
||||
_home_pos_sub.update(&_home_pos);
|
||||
|
||||
if (_home_pos_sub.update(&_home_pos)) {
|
||||
_tracker.set_home(&_home_pos);
|
||||
}
|
||||
|
||||
if (_vehicle_command_sub.updated()) {
|
||||
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
||||
@@ -632,11 +657,17 @@ Navigator::run()
|
||||
break;
|
||||
|
||||
default:
|
||||
if (rtl_activated) {
|
||||
mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
|
||||
if (_use_advanced_rtl) {
|
||||
navigation_mode_new = &_smartRtl;
|
||||
|
||||
} else {
|
||||
if (rtl_activated) {
|
||||
mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
|
||||
}
|
||||
|
||||
navigation_mode_new = &_rtl;
|
||||
}
|
||||
|
||||
navigation_mode_new = &_rtl;
|
||||
break;
|
||||
|
||||
}
|
||||
@@ -772,6 +803,11 @@ Navigator::print_status()
|
||||
PX4_INFO("Running");
|
||||
|
||||
_geofence.printStatus();
|
||||
|
||||
_tracker.dump_recent_path();
|
||||
_tracker.dump_graph();
|
||||
//_tracker.dump_path_to_home();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1219,6 +1255,25 @@ int Navigator::custom_command(int argc, char *argv[])
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LARGE);
|
||||
get_instance()->fake_traffic("UAV", 10, 1.0f, -2.0f, 10.0f, 10.0f, 0.01f, transponder_report_s::ADSB_EMITTER_TYPE_UAV);
|
||||
return 0;
|
||||
|
||||
} else if (!strcmp(argv[0], "tracker") && argc >= 2) {
|
||||
if (!strcmp(argv[1], "reset")) {
|
||||
// Deletes the entire flight graph (but not the most recent path!).
|
||||
// This may be neccessary if the environment changed heavily since system start.
|
||||
get_instance()->tracker_reset();
|
||||
|
||||
} else if (!strcmp(argv[1], "consolidate")) {
|
||||
// Consolidates the flight graph.
|
||||
// This is not required for normal operation, as it happens automatically.
|
||||
get_instance()->tracker_consolidate();
|
||||
|
||||
} else if (!strcmp(argv[1], "rewrite")) {
|
||||
// Deletes everything from the flight graph, which does not lead home.
|
||||
// This is not required for normal operation, as it happens automatically.
|
||||
get_instance()->tracker_rewrite();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
return print_usage("unknown command");
|
||||
|
||||
@@ -0,0 +1,88 @@
|
||||
Checks: '*
|
||||
,-cert-dcl50-cpp
|
||||
,-cert-err34-c
|
||||
,-cert-err58-cpp
|
||||
,-cert-msc30-c
|
||||
,-cert-msc50-cpp
|
||||
,-clang-analyzer-core.CallAndMessage
|
||||
,-clang-analyzer-core.DivideZero
|
||||
,-clang-analyzer-core.NullDereference
|
||||
,-clang-analyzer-core.UndefinedBinaryOperatorResult
|
||||
,-clang-analyzer-core.uninitialized.Assign
|
||||
,-clang-analyzer-core.VLASize
|
||||
,-clang-analyzer-cplusplus.NewDelete
|
||||
,-clang-analyzer-cplusplus.NewDeleteLeaks
|
||||
,-clang-analyzer-deadcode.DeadStores
|
||||
,-clang-analyzer-optin.cplusplus.VirtualCall
|
||||
,-clang-analyzer-optin.performance.Padding
|
||||
,-clang-analyzer-security.insecureAPI.strcpy
|
||||
,-clang-analyzer-unix.API
|
||||
,-clang-analyzer-unix.cstring.BadSizeArg
|
||||
,-clang-analyzer-unix.Malloc
|
||||
,-clang-analyzer-unix.MallocSizeof
|
||||
,-cppcoreguidelines-c-copy-assignment-signature
|
||||
,-cppcoreguidelines-interfaces-global-init
|
||||
,-cppcoreguidelines-no-malloc
|
||||
,-cppcoreguidelines-pro-bounds-array-to-pointer-decay
|
||||
,-cppcoreguidelines-pro-bounds-constant-array-index
|
||||
,-cppcoreguidelines-pro-bounds-pointer-arithmetic
|
||||
,-cppcoreguidelines-pro-type-const-cast
|
||||
,-cppcoreguidelines-pro-type-cstyle-cast
|
||||
,-cppcoreguidelines-pro-type-member-init
|
||||
,-cppcoreguidelines-pro-type-reinterpret-cast
|
||||
,-cppcoreguidelines-pro-type-union-access
|
||||
,-cppcoreguidelines-pro-type-vararg
|
||||
,-cppcoreguidelines-special-member-functions
|
||||
,-google-build-using-namespace
|
||||
,-google-explicit-constructor
|
||||
,-google-global-names-in-headers
|
||||
,-google-readability-casting
|
||||
,-google-readability-namespace-comments
|
||||
,-google-readability-todo
|
||||
,-google-runtime-int
|
||||
,-google-runtime-references
|
||||
,-llvm-header-guard
|
||||
,-llvm-include-order
|
||||
,-llvm-namespace-comment
|
||||
,-misc-incorrect-roundings
|
||||
,-misc-macro-parentheses
|
||||
,-misc-misplaced-widening-cast
|
||||
,-misc-redundant-expression
|
||||
,-misc-unconventional-assign-operator
|
||||
,-misc-unused-parameters
|
||||
,-modernize-deprecated-headers
|
||||
,-modernize-loop-convert
|
||||
,-modernize-use-auto
|
||||
,-modernize-use-bool-literals
|
||||
,-modernize-use-default-member-init
|
||||
,-modernize-use-emplace
|
||||
,-modernize-use-equals-default
|
||||
,-modernize-use-equals-delete
|
||||
,-modernize-use-override
|
||||
,-modernize-use-using
|
||||
,-performance-inefficient-string-concatenation
|
||||
,-readability-avoid-const-params-in-decls
|
||||
,-readability-else-after-return
|
||||
,-readability-implicit-bool-cast
|
||||
,-readability-inconsistent-declaration-parameter-name
|
||||
,-readability-non-const-parameter
|
||||
,-readability-redundant-declaration
|
||||
,-readability-redundant-member-init
|
||||
,-readability-simplify-boolean-expr
|
||||
'
|
||||
WarningsAsErrors: '*'
|
||||
CheckOptions:
|
||||
- key: google-readability-braces-around-statements.ShortStatementLines
|
||||
value: '1'
|
||||
- key: google-readability-function-size.BranchThreshold
|
||||
value: '600'
|
||||
- key: google-readability-function-size.LineThreshold
|
||||
value: '4000'
|
||||
- key: google-readability-function-size.StatementThreshold
|
||||
value: '4000'
|
||||
- key: readability-braces-around-statements.ShortStatementLines
|
||||
value: '1'
|
||||
- key: readability-function-size.LineThreshold
|
||||
value: '4000'
|
||||
- key: readability-function-size.StatementThreshold
|
||||
value: '4000'
|
||||
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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_module(
|
||||
MODULE modules__navigator__navigator_tests
|
||||
MAIN navigator_tests
|
||||
COMPILE_FLAGS
|
||||
-Wno-error=cast-align
|
||||
SRCS
|
||||
navigator_tests.cpp
|
||||
tracker_test.cpp
|
||||
DEPENDS
|
||||
|
||||
)
|
||||
@@ -0,0 +1,56 @@
|
||||
// Complex loop with multiple intersections
|
||||
// Also, sometimes we walk forward on the path
|
||||
|
||||
|
||||
50,24,0,
|
||||
53,24,0,
|
||||
56,24,0,
|
||||
59,24,0,
|
||||
62,25,0,
|
||||
64,27,0,
|
||||
65,30,0,
|
||||
63,32,0,
|
||||
60,33,0,
|
||||
55,33,0,
|
||||
50,32,0,
|
||||
49,30,0,
|
||||
50,28,0,
|
||||
53,27,0,
|
||||
59,28,0,
|
||||
60,30,0,
|
||||
57,32,0,
|
||||
55,33,0,
|
||||
54,35,0,
|
||||
56,37,0,
|
||||
61,36,0,
|
||||
67,35,0,
|
||||
75,31,0,
|
||||
74,26,0,
|
||||
72,20,0,
|
||||
65,18,0,
|
||||
58,21,0,
|
||||
56,24,0,
|
||||
55,26,0,
|
||||
53,27,0,
|
||||
52,30,0,
|
||||
50,32,0,
|
||||
47,34,0,
|
||||
43,35,0,
|
||||
39,33,0,
|
||||
36,34,0,
|
||||
34,36,0,
|
||||
32,38,0,
|
||||
32,41,0,
|
||||
35,43,0,
|
||||
39,43,0,
|
||||
43,43,0,
|
||||
47,43,0,
|
||||
50,43,0,
|
||||
53,41,0,
|
||||
57,40,0,
|
||||
62,40,0,
|
||||
65,39,0,
|
||||
68,38,0,
|
||||
67,35,0,
|
||||
66,33,0,
|
||||
67,31,0,
|
||||
@@ -0,0 +1,56 @@
|
||||
// This is taken from an actual flight test in the simulator.
|
||||
// 1. On PX4 command line: navigator status
|
||||
// 2. copy the graph positions and paste to file
|
||||
// 3. use multi-cursor-editing to extract only coordinates
|
||||
// 4. reverse lines in unix: tail -r path
|
||||
|
||||
0, 0, -2,
|
||||
0, 0, -3,
|
||||
1, 0, -4,
|
||||
2, 0, -4,
|
||||
3, 0, -4,
|
||||
4, 0, -4,
|
||||
5, 0, -4,
|
||||
6, 0, -4,
|
||||
7, 0, -4,
|
||||
8, 0, -4,
|
||||
9, 0, -4,
|
||||
10, 0, -4,
|
||||
11, 0, -4,
|
||||
12, 0, -4,
|
||||
13, 0, -4,
|
||||
14, 0, -4,
|
||||
14, 1, -4,
|
||||
13, 2, -4,
|
||||
13, 3, -4,
|
||||
12, 4, -4,
|
||||
11, 5, -4,
|
||||
10, 6, -4,
|
||||
9, 7, -4,
|
||||
7, 5, -4,
|
||||
7, 3, -4,
|
||||
6, 1, -4,
|
||||
6, 0, -4,
|
||||
6, -1, -4,
|
||||
6, -2, -4,
|
||||
6, -3, -4,
|
||||
6, -4, -4,
|
||||
6, -5, -4,
|
||||
6, -6, -4,
|
||||
6, -7, -4,
|
||||
6, -8, -4,
|
||||
7, -8, -4,
|
||||
8, -7, -4,
|
||||
9, -6, -4,
|
||||
10, -5, -4,
|
||||
11, -3, -4,
|
||||
12, -2, -4,
|
||||
13, 0, -4,
|
||||
15, 2, -4,
|
||||
15, 3, -4,
|
||||
16, 4, -4,
|
||||
16, 5, -4,
|
||||
17, 6, -4,
|
||||
17, 7, -4,
|
||||
18, 8, -4,
|
||||
19, 9, -4,
|
||||
@@ -0,0 +1,53 @@
|
||||
// A path that contains up to 4 crossings of the same point
|
||||
|
||||
|
||||
51,24,0,
|
||||
54,24,0,
|
||||
58,24,0,
|
||||
62,25,0,
|
||||
66,27,0,
|
||||
69,30,0,
|
||||
71,34,0,
|
||||
67,37,0,
|
||||
61,38,0,
|
||||
54,37,0,
|
||||
51,36,0,
|
||||
49,34,0,
|
||||
49,31,0,
|
||||
51,29,0,
|
||||
53,27,0,
|
||||
56,26,0,
|
||||
58,24,0,
|
||||
61,22,0,
|
||||
62,19,0,
|
||||
60,17,0,
|
||||
55,17,0,
|
||||
52,19,0,
|
||||
54,21,0,
|
||||
57,22,0,
|
||||
58,24,0,
|
||||
60,27,0,
|
||||
62,30,0,
|
||||
64,32,0,
|
||||
62,35,0,
|
||||
61,38,0,
|
||||
59,41,0,
|
||||
58,45,0,
|
||||
63,46,0,
|
||||
69,45,0,
|
||||
69,41,0,
|
||||
65,40,0,
|
||||
61,38,0,
|
||||
57,33,0,
|
||||
54,30,0,
|
||||
51,29,0,
|
||||
47,26,0,
|
||||
44,28,0,
|
||||
47,29,0,
|
||||
51,29,0,
|
||||
52,32,0,
|
||||
54,35,0,
|
||||
57,36,0,
|
||||
61,38,0,
|
||||
55,40,0,
|
||||
50,41,0,
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,15 @@
|
||||
|
||||
/**
|
||||
* Navigator unit tests. Run the tests as follows:
|
||||
* nsh> navigator_tests
|
||||
*
|
||||
*/
|
||||
|
||||
//#include <systemlib/err.h>
|
||||
|
||||
#include "tracker_test.h"
|
||||
|
||||
extern "C" __EXPORT int navigator_tests_main(int argc, char *argv[])
|
||||
{
|
||||
return trackerTest() ? 0 : -1;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
// A curve with no intersection
|
||||
|
||||
45,24,0,
|
||||
48,24,0,
|
||||
51,25,0,
|
||||
54,27,0,
|
||||
55,29,0,
|
||||
52,31,0,
|
||||
47,31,0,
|
||||
43,29,0,
|
||||
41,27,0,
|
||||
39,25,0,
|
||||
39,22,0,
|
||||
42,20,0,
|
||||
48,18,0,
|
||||
57,17,0,
|
||||
@@ -0,0 +1,27 @@
|
||||
62,24,0,
|
||||
71,28,0,
|
||||
73,37,0,
|
||||
60,41,0,
|
||||
42,39,0,
|
||||
39,31,0,
|
||||
52,34,0,
|
||||
52,40,0,
|
||||
57,41,0,
|
||||
62,41,0,
|
||||
66,39,0,
|
||||
71,38,0,
|
||||
73,36,0,
|
||||
72,34,0,
|
||||
71,30,0,
|
||||
66,26,0,
|
||||
69,28,0,
|
||||
72,32,0,
|
||||
73,35,0,
|
||||
70,38,0,
|
||||
65,40,0,
|
||||
58,41,0,
|
||||
49,40,0,
|
||||
45,39,0,
|
||||
41,36,0,
|
||||
28,37,0,
|
||||
25,33,0,
|
||||
@@ -0,0 +1,12 @@
|
||||
// A path with one simple loop
|
||||
|
||||
46,24,0,
|
||||
49,24,0,
|
||||
52,24,0,
|
||||
55,25,0,
|
||||
55,28,0,
|
||||
51,28,0,
|
||||
47,27,0,
|
||||
49,24,0,
|
||||
51,21,0,
|
||||
54,21,0,
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,15 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
// Usually we want progress on every single return path query.
|
||||
// We can relax this constraint for debugging.
|
||||
#define TRACKER_MAX_NO_PROGRESS 3
|
||||
|
||||
#define TRACKER_TEST_LOOKAHEAD
|
||||
|
||||
#if defined(_POSIX_VERSION)
|
||||
// Omit some tests on small systems to save flash memory
|
||||
#define TRACKER_TEST_LONG_PATHS
|
||||
#endif
|
||||
|
||||
bool trackerTest();
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2014-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
|
||||
@@ -147,3 +147,20 @@ PARAM_DEFINE_INT32(RTL_CONE_ANG, 0);
|
||||
* @group Return To Land
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
|
||||
|
||||
/**
|
||||
* RTL Advanced to RTL classic fallback delay
|
||||
*
|
||||
* If the vehicle makes no progress in advanced RTL for this timespan for whatever reason, the system falls back to basic RTL mode.
|
||||
* Note that there may be long straight lines in the return path. This delay should be large enough to allow flying along such lines.
|
||||
* If set to -1 the system will not switch to basic RTL but loiter.
|
||||
*
|
||||
* @unit s
|
||||
* @min -1
|
||||
* @max 300
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group Return To Land
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_FALLBCK_DLY, 120f);
|
||||
|
||||
|
||||
@@ -0,0 +1,220 @@
|
||||
/**
|
||||
* @file rcrecover.cpp
|
||||
* RC recovery navigation mode
|
||||
*/
|
||||
|
||||
#include "smart_rtl.h"
|
||||
|
||||
#include <lib/systemlib/mavlink_log.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
#include "navigator.h"
|
||||
|
||||
#define DELAY_SIGMA 0.01f
|
||||
|
||||
// #define DEBUG_RTL
|
||||
|
||||
SmartRTL::SmartRTL(Navigator *navigator) :
|
||||
MissionBlock(navigator),
|
||||
ModuleParams(navigator)
|
||||
{
|
||||
// load initial params
|
||||
updateParams();
|
||||
|
||||
// initial reset
|
||||
on_inactive();
|
||||
}
|
||||
|
||||
void SmartRTL::on_inactive()
|
||||
{
|
||||
_tracker = nullptr;
|
||||
deadline = HRT_ABSTIME_MAX;
|
||||
}
|
||||
|
||||
void SmartRTL::on_activation()
|
||||
{
|
||||
// For safety reasons don't go into RTL if landed
|
||||
if (_navigator->get_land_detected()->landed) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Return To Land: not available when landed");
|
||||
return;
|
||||
}
|
||||
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Return To Land: return along recorded path");
|
||||
|
||||
_tracker = _navigator->get_tracker();
|
||||
_tracker->dump_graph();
|
||||
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
// Init return path and setpoint triplet
|
||||
init_setpoint(pos_sp_triplet->current);
|
||||
pos_sp_triplet->current.valid = _tracker->init_return_path_global(
|
||||
current_return_context,
|
||||
pos_sp_triplet->current.lat,
|
||||
pos_sp_triplet->current.lon,
|
||||
pos_sp_triplet->current.alt);
|
||||
|
||||
if (pos_sp_triplet->current.valid) {
|
||||
TRACKER_DBG("got return context");
|
||||
}
|
||||
|
||||
_tracker->dump_graph();
|
||||
|
||||
advance_setpoint_triplet(pos_sp_triplet);
|
||||
update_deadline();
|
||||
}
|
||||
|
||||
void SmartRTL::on_active()
|
||||
{
|
||||
// If the tracker fails, do the same as if the deadline was reached
|
||||
if (_tracker && _tracker->get_graph_fault()) {
|
||||
PX4_ERR("the flight graph became inconsistent and return path was lost");
|
||||
deadline = 0;
|
||||
}
|
||||
|
||||
if (deadline <= hrt_absolute_time()) {
|
||||
_tracker = nullptr;
|
||||
deadline = HRT_ABSTIME_MAX;
|
||||
|
||||
if (land_after_deadline) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Return To Land: landing");
|
||||
TRACKER_DBG("proceed to land");
|
||||
|
||||
// Perform landing
|
||||
set_land_item(&_mission_item, true);
|
||||
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->next.valid = false;
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Return To Land: fallback to legacy mode");
|
||||
TRACKER_DBG("fall back to legacy RTL");
|
||||
_navigator->set_rtl_variant(false);
|
||||
}
|
||||
|
||||
} else if (_tracker && _tracker->is_context_close_to_head(current_return_context)) {
|
||||
if (advance_setpoint_triplet(_navigator->get_position_setpoint_triplet())) {
|
||||
// We made progress, update the deadline.
|
||||
update_deadline();
|
||||
|
||||
} else {
|
||||
// If the return path is empty, discard the tracker so we don't make further unneccessary checks.
|
||||
_tracker = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(DEBUG_RTL)
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
if (now >= next_log) {
|
||||
next_log = now + (hrt_abstime)(1 * 1e6f);
|
||||
position_setpoint_s &sp = _navigator->get_position_setpoint_triplet()->next;
|
||||
PX4_WARN("next waypoint: %.0f°, %.2fm", (double)bearing_to_setpoint(sp), (double)distance_to_setpoint(sp));
|
||||
}
|
||||
|
||||
#endif // DEBUG_RTL
|
||||
}
|
||||
|
||||
void SmartRTL::update_deadline()
|
||||
{
|
||||
updateParams();
|
||||
|
||||
land_after_deadline = _tracker->is_context_close_to_home(current_return_context);
|
||||
float delay = land_after_deadline ? _param_rtl_land_delay.get() : _param_rtl_fallbck_dly.get();
|
||||
|
||||
if (delay < 0) {
|
||||
deadline = HRT_ABSTIME_MAX;
|
||||
|
||||
} else {
|
||||
deadline = hrt_absolute_time() + (hrt_abstime)(delay * 1e6f);
|
||||
}
|
||||
}
|
||||
|
||||
void SmartRTL::init_setpoint(position_setpoint_s &sp)
|
||||
{
|
||||
sp.valid = false;
|
||||
sp.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
sp.position_valid = false;
|
||||
sp.x = 0;
|
||||
sp.y = 0;
|
||||
sp.z = 0;
|
||||
sp.lat = 0;
|
||||
sp.lon = 0;
|
||||
sp.alt = 0;
|
||||
sp.acceptance_radius = 1;
|
||||
sp.cruising_speed = _navigator->get_cruising_speed();
|
||||
sp.cruising_throttle = _navigator->get_cruising_throttle();
|
||||
sp.loiter_radius = _navigator->get_loiter_radius();
|
||||
sp.loiter_direction = 1;
|
||||
}
|
||||
|
||||
float SmartRTL::distance_to_setpoint(position_setpoint_s &sp)
|
||||
{
|
||||
float distance_xy = 0.f;
|
||||
float distance_z = 0.f;
|
||||
|
||||
get_distance_to_point_global_wgs84(
|
||||
_navigator->get_global_position()->lat,
|
||||
_navigator->get_global_position()->lon,
|
||||
_navigator->get_global_position()->alt,
|
||||
sp.lat, sp.lon, sp.alt,
|
||||
&distance_xy, &distance_z);
|
||||
|
||||
return sqrtf(distance_xy * distance_xy + distance_z * distance_z);
|
||||
}
|
||||
|
||||
float SmartRTL::bearing_to_setpoint(position_setpoint_s &sp)
|
||||
{
|
||||
float bearing = get_bearing_to_next_waypoint(
|
||||
_navigator->get_global_position()->lat,
|
||||
_navigator->get_global_position()->lon,
|
||||
sp.lat, sp.lon) * 180.f / (float)M_PI;
|
||||
|
||||
if (bearing <= 0) {
|
||||
bearing += 360;
|
||||
}
|
||||
|
||||
return bearing;
|
||||
}
|
||||
|
||||
void SmartRTL::dump_setpoint(const char *name, position_setpoint_s &sp)
|
||||
{
|
||||
TRACKER_DBG("%s setpoint is lat %f lon %f alt %f, distance %f, %s", name,
|
||||
sp.lat, sp.lon, (double)sp.alt,
|
||||
(double)distance_to_setpoint(sp), sp.valid ? "valid" : "invalid");
|
||||
}
|
||||
|
||||
bool SmartRTL::advance_setpoint_triplet(position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
// Shift setpoints if possible
|
||||
if (pos_sp_triplet->next.valid) {
|
||||
if (pos_sp_triplet->current.valid) {
|
||||
pos_sp_triplet->previous = pos_sp_triplet->current;
|
||||
}
|
||||
|
||||
pos_sp_triplet->current = pos_sp_triplet->next;
|
||||
current_return_context = next_return_context;
|
||||
|
||||
} else {
|
||||
next_return_context = current_return_context;
|
||||
}
|
||||
|
||||
// Load next setpoint
|
||||
init_setpoint(pos_sp_triplet->next);
|
||||
pos_sp_triplet->next.valid = _tracker->advance_return_path_global(next_return_context,
|
||||
pos_sp_triplet->next.lat,
|
||||
pos_sp_triplet->next.lon,
|
||||
pos_sp_triplet->next.alt);
|
||||
|
||||
// Apply updated setpoint triplet
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
|
||||
//dump_setpoint("previous", pos_sp_triplet->previous);
|
||||
//dump_setpoint("current", pos_sp_triplet->current);
|
||||
//dump_setpoint("next", pos_sp_triplet->next);
|
||||
|
||||
return pos_sp_triplet->next.valid && !_tracker->is_same_pos(current_return_context, next_return_context);
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
#include "tracker.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class SmartRTL : public MissionBlock, public ModuleParams
|
||||
{
|
||||
public:
|
||||
SmartRTL(Navigator *navigator);
|
||||
~SmartRTL() = default;
|
||||
|
||||
void on_inactive() override;
|
||||
void on_activation() override;
|
||||
void on_active() override;
|
||||
|
||||
private:
|
||||
// Inits a setpoint
|
||||
void init_setpoint(position_setpoint_s &sp);
|
||||
|
||||
// Updates the deadline from the current time and parameters.
|
||||
void update_deadline();
|
||||
|
||||
// Returns the current distance/bearing to a setpoint
|
||||
float distance_to_setpoint(position_setpoint_s &sp);
|
||||
float bearing_to_setpoint(position_setpoint_s &sp);
|
||||
|
||||
// Prints information about the specified setpoint
|
||||
void dump_setpoint(const char *name, position_setpoint_s &sp);
|
||||
|
||||
// Advances the setpoint triplet by loading the next position from the return path.
|
||||
// Returns true if progress was made.
|
||||
bool advance_setpoint_triplet(position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
Tracker *_tracker;
|
||||
|
||||
Tracker::path_finding_context_t current_return_context;
|
||||
Tracker::path_finding_context_t next_return_context;
|
||||
|
||||
hrt_abstime deadline{HRT_ABSTIME_MAX}; // This deadline makes sure that progress is made.
|
||||
bool land_after_deadline; // If true and the deadline is reached, land, otherwise, fall back to basic RTL.
|
||||
|
||||
hrt_abstime next_log{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RTL_FALLBCK_DLY>) _param_rtl_fallbck_dly,
|
||||
(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay
|
||||
)
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -122,6 +122,7 @@ const struct {
|
||||
|
||||
/* external tests */
|
||||
{"commander", commander_tests_main, 0},
|
||||
{"navigator", navigator_tests_main, 0},
|
||||
{"controllib", controllib_test_main, 0},
|
||||
{"mavlink", mavlink_tests_main, 0},
|
||||
#ifdef __PX4_NUTTX
|
||||
|
||||
@@ -92,6 +92,7 @@ extern int test_versioning(int argc, char *argv[]);
|
||||
|
||||
/* external */
|
||||
extern int commander_tests_main(int argc, char *argv[]);
|
||||
extern int navigator_tests_main(int argc, char *argv[]);
|
||||
extern int mavlink_tests_main(int argc, char *argv[]);
|
||||
extern int controllib_test_main(int argc, char *argv[]);
|
||||
extern int uorb_tests_main(int argc, char *argv[]);
|
||||
|
||||
Reference in New Issue
Block a user