mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-10 09:51:36 +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
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user