forked from Archive/PX4-Autopilot
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
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
|
||||||
|
|
@ -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_matrix
|
||||||
microbench_uorb
|
microbench_uorb
|
||||||
mixer
|
mixer
|
||||||
|
navigator
|
||||||
param
|
param
|
||||||
parameters
|
parameters
|
||||||
perf
|
perf
|
||||||
|
|
|
||||||
|
|
@ -57,6 +57,8 @@ __BEGIN_DECLS
|
||||||
*/
|
*/
|
||||||
typedef uint64_t hrt_abstime;
|
typedef uint64_t hrt_abstime;
|
||||||
|
|
||||||
|
#define HRT_ABSTIME_MAX UINT64_MAX
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Callout function type.
|
* Callout function type.
|
||||||
*
|
*
|
||||||
|
|
|
||||||
|
|
@ -34,6 +34,8 @@
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE modules__navigator
|
MODULE modules__navigator
|
||||||
MAIN navigator
|
MAIN navigator
|
||||||
|
COMPILE_FLAGS
|
||||||
|
-Wno-error=cast-align
|
||||||
SRCS
|
SRCS
|
||||||
navigator_main.cpp
|
navigator_main.cpp
|
||||||
navigator_mode.cpp
|
navigator_mode.cpp
|
||||||
|
|
@ -49,8 +51,14 @@ px4_add_module(
|
||||||
enginefailure.cpp
|
enginefailure.cpp
|
||||||
gpsfailure.cpp
|
gpsfailure.cpp
|
||||||
follow_target.cpp
|
follow_target.cpp
|
||||||
|
smart_rtl.cpp
|
||||||
|
tracker.cpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
git_ecl
|
git_ecl
|
||||||
ecl_geo
|
ecl_geo
|
||||||
landing_slope
|
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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
@ -51,7 +51,9 @@
|
||||||
#include "mission.h"
|
#include "mission.h"
|
||||||
#include "navigator_mode.h"
|
#include "navigator_mode.h"
|
||||||
#include "rtl.h"
|
#include "rtl.h"
|
||||||
|
#include "smart_rtl.h"
|
||||||
#include "takeoff.h"
|
#include "takeoff.h"
|
||||||
|
#include "tracker.h"
|
||||||
|
|
||||||
#include "navigation.h"
|
#include "navigation.h"
|
||||||
|
|
||||||
|
|
@ -81,7 +83,7 @@
|
||||||
/**
|
/**
|
||||||
* Number of navigation modes that need on_active/on_inactive calls
|
* 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
|
class Navigator : public ModuleBase<Navigator>, public ModuleParams
|
||||||
|
|
@ -169,6 +171,11 @@ public:
|
||||||
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
|
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
|
||||||
float get_loiter_radius() { return _param_nav_loiter_rad.get(); }
|
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
|
* Returns the default acceptance radius defined by the parameter
|
||||||
*/
|
*/
|
||||||
|
|
@ -285,6 +292,12 @@ public:
|
||||||
|
|
||||||
bool abort_landing();
|
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
|
// Param access
|
||||||
float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
|
float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
|
||||||
float get_takeoff_min_alt() const { return _param_mis_takeoff_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 */
|
Geofence _geofence; /**< class that handles the geofence */
|
||||||
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
|
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 _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_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 _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 _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 */
|
Mission _mission; /**< class that handles the missions */
|
||||||
Loiter _loiter; /**< class that handles loiter */
|
Loiter _loiter; /**< class that handles loiter */
|
||||||
Takeoff _takeoff; /**< class for handling takeoff commands */
|
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 */
|
PrecLand _precland; /**< class for handling precision land commands */
|
||||||
RTL _rtl; /**< class that handles RTL */
|
RTL _rtl; /**< class that handles RTL */
|
||||||
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
|
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
|
||||||
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
|
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
|
||||||
FollowTarget _follow_target;
|
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 */
|
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -86,7 +86,8 @@ Navigator::Navigator() :
|
||||||
_rtl(this),
|
_rtl(this),
|
||||||
_engineFailure(this),
|
_engineFailure(this),
|
||||||
_gpsFailure(this),
|
_gpsFailure(this),
|
||||||
_follow_target(this)
|
_follow_target(this),
|
||||||
|
_smartRtl(this)
|
||||||
{
|
{
|
||||||
/* Create a list of our possible navigation types */
|
/* Create a list of our possible navigation types */
|
||||||
_navigation_mode_array[0] = &_mission;
|
_navigation_mode_array[0] = &_mission;
|
||||||
|
|
@ -98,6 +99,7 @@ Navigator::Navigator() :
|
||||||
_navigation_mode_array[6] = &_land;
|
_navigation_mode_array[6] = &_land;
|
||||||
_navigation_mode_array[7] = &_precland;
|
_navigation_mode_array[7] = &_precland;
|
||||||
_navigation_mode_array[8] = &_follow_target;
|
_navigation_mode_array[8] = &_follow_target;
|
||||||
|
_navigation_mode_array[9] = &_smartRtl;
|
||||||
|
|
||||||
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
|
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
|
||||||
_handle_reverse_delay = param_find("VT_B_REV_DEL");
|
_handle_reverse_delay = param_find("VT_B_REV_DEL");
|
||||||
|
|
@ -176,7 +178,18 @@ Navigator::run()
|
||||||
} else {
|
} else {
|
||||||
if (fds[0].revents & POLLIN) {
|
if (fds[0].revents & POLLIN) {
|
||||||
/* success, local pos is available */
|
/* 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();
|
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();
|
_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()) {
|
if (_vehicle_command_sub.updated()) {
|
||||||
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
||||||
|
|
@ -632,11 +657,17 @@ Navigator::run()
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
if (rtl_activated) {
|
if (_use_advanced_rtl) {
|
||||||
mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
|
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;
|
break;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
@ -772,6 +803,11 @@ Navigator::print_status()
|
||||||
PX4_INFO("Running");
|
PX4_INFO("Running");
|
||||||
|
|
||||||
_geofence.printStatus();
|
_geofence.printStatus();
|
||||||
|
|
||||||
|
_tracker.dump_recent_path();
|
||||||
|
_tracker.dump_graph();
|
||||||
|
//_tracker.dump_path_to_home();
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -1219,6 +1255,25 @@ int Navigator::custom_command(int argc, char *argv[])
|
||||||
transponder_report_s::ADSB_EMITTER_TYPE_LARGE);
|
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);
|
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;
|
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");
|
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,
|
||||||
|
|
@ -0,0 +1,630 @@
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Flight path tracker tests
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "tracker_test.h"
|
||||||
|
|
||||||
|
#include "../tracker.h"
|
||||||
|
#include "unit_test.h"
|
||||||
|
#include <math.h> // sqrt
|
||||||
|
#include <cstring> // std::strcat
|
||||||
|
#include <climits> // INT_MAX
|
||||||
|
#include <lib/mathlib/math/Limits.hpp> // std::min, max
|
||||||
|
|
||||||
|
|
||||||
|
#define DEFINE_TEST(testName, ...) \
|
||||||
|
const size_t testName ## Ret[] = { __VA_ARGS__ }; \
|
||||||
|
const int testName[] =
|
||||||
|
|
||||||
|
#define USE_TEST(testName) { .name = #testName, .path = testName, .path_size = sizeof(testName) / sizeof(testName[0]), .ret = testName ## Ret, .ret_size = sizeof(testName ## Ret) / sizeof(testName ## Ret[0]) }
|
||||||
|
|
||||||
|
|
||||||
|
DEFINE_TEST(noLoop, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0)
|
||||||
|
{
|
||||||
|
#include "no_loop.path"
|
||||||
|
};
|
||||||
|
|
||||||
|
DEFINE_TEST(simpleLoop, 8, 1, 0)
|
||||||
|
{
|
||||||
|
#include "simple_loop.path"
|
||||||
|
};
|
||||||
|
|
||||||
|
// aka: 21 9 31 13 2
|
||||||
|
DEFINE_TEST(complexLoop, 50, 49, 20, 19, 18, 17, 10, 30, 29, 28, 27, 1, 0)
|
||||||
|
{
|
||||||
|
#include "complex_loop.path"
|
||||||
|
};
|
||||||
|
|
||||||
|
// aka: 8
|
||||||
|
// aka: 29 2
|
||||||
|
// aka: 36 16
|
||||||
|
DEFINE_TEST(largeNodes, 48, 47, 28, 27, 26, 25, 24, 1, 0)
|
||||||
|
{
|
||||||
|
#include "large_nodes.path"
|
||||||
|
};
|
||||||
|
|
||||||
|
// aka: 14
|
||||||
|
DEFINE_TEST(fromSim, 48, 47, 46, 45, 44, 43, 42, 41, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0)
|
||||||
|
{
|
||||||
|
#include "from_sim.path"
|
||||||
|
};
|
||||||
|
|
||||||
|
// aka:
|
||||||
|
DEFINE_TEST(simpleJumps, 25, 24, 4, 3, 2, 1, 0)
|
||||||
|
{
|
||||||
|
#include "simple_jumps.path"
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef TRACKER_TEST_LONG_PATHS
|
||||||
|
DEFINE_TEST(longPath1)
|
||||||
|
{
|
||||||
|
#include "long_path1.path"
|
||||||
|
};
|
||||||
|
|
||||||
|
DEFINE_TEST(longPath2)
|
||||||
|
{
|
||||||
|
#include "long_path2.path"
|
||||||
|
};
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
class TrackerTest : public UnitTest
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
TrackerTest() = default;
|
||||||
|
virtual ~TrackerTest() = default;
|
||||||
|
|
||||||
|
bool run_tests();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
struct line_test_t {
|
||||||
|
Tracker::ipos_t delta1, end1;
|
||||||
|
Tracker::ipos_t delta2, end2;
|
||||||
|
Tracker::ipos_t result;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct test_t {
|
||||||
|
const char *name;
|
||||||
|
const int *path;
|
||||||
|
const size_t path_size;
|
||||||
|
const size_t *ret;
|
||||||
|
const size_t ret_size;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct return_state_t {
|
||||||
|
const test_t *test;
|
||||||
|
size_t target_index;
|
||||||
|
int no_progress;
|
||||||
|
float x, y, z;
|
||||||
|
};
|
||||||
|
|
||||||
|
float get_distance(float x1, float y1, float z1, float x2, float y2, float z2);
|
||||||
|
|
||||||
|
// Returns the distance of the specified point to a line in the specified test
|
||||||
|
float distance_to_line(float x, float y, float z, const test_t *test, size_t line_end_index);
|
||||||
|
|
||||||
|
// Registers the progress represented by the new position.
|
||||||
|
// Returns false if the new position is no longer on the expected return path.
|
||||||
|
bool advance_state(return_state_t &state, float new_x, float new_y, float new_z, float accuracy);
|
||||||
|
|
||||||
|
// Returns true if the return state has reached the home position
|
||||||
|
bool detect_completion(return_state_t &state);
|
||||||
|
|
||||||
|
// Initializes the return path supervision from any given position
|
||||||
|
return_state_t init_return_state(float x, float y, float z, const test_t *test);
|
||||||
|
|
||||||
|
// Makes the tracker return along the shortest path home and tests whether it chooses the right path.
|
||||||
|
// Returns false if the test fails and fills msg with an appropriate error message.
|
||||||
|
bool try_return_supervised(Tracker &tracker, const test_t *test, char *msg);
|
||||||
|
|
||||||
|
// Makes the tracker return along the shortest path home. Use this if there is no well known return path.
|
||||||
|
// Returns false if the test fails and fills msg with an appropriate error message.
|
||||||
|
bool try_return_unsupervised(Tracker &tracker, const test_t *test, float home_x, float home_y, float home_z,
|
||||||
|
float home_to_path_dist, char *msg);
|
||||||
|
|
||||||
|
// Simulates a flight along the defined test path,
|
||||||
|
// and then returns on the shortest path while verifying that
|
||||||
|
// the tracker recommends the expected return path.
|
||||||
|
bool fly_and_return_test();
|
||||||
|
|
||||||
|
// Simulates a flight along the defined test path,
|
||||||
|
// returns half of the shortest path, flies to some other pseudorandom (deterministic)
|
||||||
|
// position in the graph, and then returns along the shortest path from there.
|
||||||
|
bool fly_and_leave_return_path_test();
|
||||||
|
|
||||||
|
// Simulates flight along the defined test path, selects some pseudorandom
|
||||||
|
// new home along the path and returns to that new home
|
||||||
|
bool fly_and_change_home_test();
|
||||||
|
|
||||||
|
// Takes several performance measurements
|
||||||
|
bool performance_test();
|
||||||
|
|
||||||
|
// Tests if the minimum line-to-line delta is calculated correctly
|
||||||
|
bool line_to_line_test();
|
||||||
|
|
||||||
|
// Enables graph rewriting in some tests
|
||||||
|
bool rewrite_graph = false;
|
||||||
|
|
||||||
|
Tracker _tracker;
|
||||||
|
|
||||||
|
// An array of flight paths and their correct return paths
|
||||||
|
// We can do various tests on each of these.
|
||||||
|
static const test_t test_cases[];
|
||||||
|
|
||||||
|
// An array of line pairs and the shortest delta between them.
|
||||||
|
static line_test_t line_test_cases[];
|
||||||
|
};
|
||||||
|
|
||||||
|
const TrackerTest::test_t TrackerTest::test_cases[] = {
|
||||||
|
USE_TEST(noLoop),
|
||||||
|
USE_TEST(simpleLoop),
|
||||||
|
USE_TEST(complexLoop),
|
||||||
|
USE_TEST(largeNodes),
|
||||||
|
USE_TEST(fromSim),
|
||||||
|
USE_TEST(simpleJumps),
|
||||||
|
#ifdef TRACKER_TEST_LONG_PATHS
|
||||||
|
USE_TEST(longPath1),
|
||||||
|
USE_TEST(longPath2)
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
TrackerTest::line_test_t TrackerTest::line_test_cases[] = {
|
||||||
|
// test cases generated with Matlab script Tools/Matlab/lineToLineTest.m
|
||||||
|
{ .delta1 = { 0, 0, 0 }, .end1 = { 0, 0, 0 }, .delta2 = { 0, 0, 0 }, .end2 = { 0, 0, 0 }, .result = { 0, 0, 0 } },
|
||||||
|
{ .delta1 = { 4, 2, -1 }, .end1 = { 2, 1, -2 }, .delta2 = { 6, 2, 2 }, .end2 = { 2, 3, 0 }, .result = { -1, 2, 1 } },
|
||||||
|
{ .delta1 = { 4, 2, -1 }, .end1 = { 2, 1, -2 }, .delta2 = { 6, 2, 2 }, .end2 = {5, 4, 4 }, .result = { 0, 2, 3 } },
|
||||||
|
{ .delta1 = { 4, 2, -1 }, .end1 = { 2, 2, -2 }, .delta2 = { 4, 1, 1 }, .end2 = {5, 4, 4 }, .result = { -1, 1, 5 } },
|
||||||
|
{ .delta1 = { 4, 2, -1 }, .end1 = { 2, 1, -2 }, .delta2 = { 3, 1, 1 }, .end2 = {-1, 2, -2 }, .result = { -1, 2, 0 } },
|
||||||
|
//{ .delta1 = { 4, 2, -1 }, .end1 = { 2, 1, -2 }, .delta2 = { -2, 3, 2 }, .end2 = {-1, 2, -2 }, .result = { 0, 1, -1 } }, with rounding temp_delta
|
||||||
|
{ .delta1 = { 4, 2, -1 }, .end1 = { 2, 1, -2 }, .delta2 = { -2, 3, 2 }, .end2 = {-1, 2, -2 }, .result = { -1, 0, -1 } }, // without rounding temp_delta
|
||||||
|
{ .delta1 = { 4, 2, -1 }, .end1 = { 2, 1, -2 }, .delta2 = { -2, 3, 2 }, .end2 = {-3, 5, 0 }, .result = { -1, 2, 0 } },
|
||||||
|
{ .delta1 = { 4, 2, -1 }, .end1 = { 6, 3, -3 }, .delta2 = { -2, 3, 2 }, .end2 = {-3, 5, 0 }, .result = { -3, 1, 0 } }
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
float TrackerTest::get_distance(float x1, float y1, float z1, float x2, float y2, float z2)
|
||||||
|
{
|
||||||
|
return sqrtf((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
float TrackerTest::distance_to_line(float x, float y, float z, const test_t *test, size_t line_end_index)
|
||||||
|
{
|
||||||
|
float line_end_x = test->path[test->ret[line_end_index] * 3];
|
||||||
|
float line_end_y = test->path[test->ret[line_end_index] * 3 + 1];
|
||||||
|
float line_end_z = test->path[test->ret[line_end_index] * 3 + 2];
|
||||||
|
float base_x = test->path[test->ret[line_end_index - 1] * 3] - line_end_x;
|
||||||
|
float base_y = test->path[test->ret[line_end_index - 1] * 3 + 1] - line_end_y;
|
||||||
|
float base_z = test->path[test->ret[line_end_index - 1] * 3 + 2] - line_end_z;
|
||||||
|
float span_x = x - line_end_x;
|
||||||
|
float span_y = y - line_end_y;
|
||||||
|
float span_z = z - line_end_z;
|
||||||
|
|
||||||
|
float cross_x = base_y * span_z - base_z * span_y;
|
||||||
|
float cross_y = base_z * span_x - base_x * span_z;
|
||||||
|
float cross_z = base_x * span_y - base_y * span_x;
|
||||||
|
|
||||||
|
return sqrtf(cross_x * cross_x + cross_y * cross_y + cross_z * cross_z) / sqrtf(base_x * base_x + base_y * base_y +
|
||||||
|
base_z
|
||||||
|
* base_z);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::advance_state(return_state_t &state, float new_x, float new_y, float new_z, float accuracy)
|
||||||
|
{
|
||||||
|
size_t target_index = state.target_index;
|
||||||
|
state.no_progress++;
|
||||||
|
|
||||||
|
do {
|
||||||
|
float target_x = state.test->path[state.test->ret[target_index] * 3];
|
||||||
|
float target_y = state.test->path[state.test->ret[target_index] * 3 + 1];
|
||||||
|
float target_z = state.test->path[state.test->ret[target_index] * 3 + 2];
|
||||||
|
|
||||||
|
float old_dist_to_target = get_distance(target_x, target_y, target_z, state.x, state.y, state.z);
|
||||||
|
float new_dist_to_target = get_distance(target_x, target_y, target_z, new_x, new_y, new_z);
|
||||||
|
|
||||||
|
if (new_dist_to_target <= old_dist_to_target) {
|
||||||
|
if (new_dist_to_target < old_dist_to_target) {
|
||||||
|
state.no_progress = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (distance_to_line(new_x, new_y, new_z, state.test, target_index) <= accuracy) {
|
||||||
|
state.target_index = target_index;
|
||||||
|
state.x = new_x;
|
||||||
|
state.y = new_y;
|
||||||
|
state.z = new_z;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} while (++target_index < state.test->ret_size);
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::detect_completion(return_state_t &state)
|
||||||
|
{
|
||||||
|
float home_x = state.test->path[0];
|
||||||
|
float home_y = state.test->path[1];
|
||||||
|
float home_z = state.test->path[2];
|
||||||
|
|
||||||
|
float dist_to_home = get_distance(state.x, state.y, state.z, home_x, home_y, home_z);
|
||||||
|
return dist_to_home < 2 * Tracker::GRID_SIZE; // tracking accuracy at home is twice the grid size
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TrackerTest::return_state_t TrackerTest::init_return_state(float x, float y, float z, const test_t *test)
|
||||||
|
{
|
||||||
|
return {
|
||||||
|
.test = test,
|
||||||
|
.target_index = 0,
|
||||||
|
.no_progress = 0,
|
||||||
|
.x = x,
|
||||||
|
.y = y,
|
||||||
|
.z = z
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#define inner_assert(value, ...) \
|
||||||
|
do { \
|
||||||
|
if (!(value)) { \
|
||||||
|
sprintf(msg, __VA_ARGS__); \
|
||||||
|
std::strcat(msg, ", on test "); \
|
||||||
|
std::strcat(msg, test->name); \
|
||||||
|
return false; \
|
||||||
|
} \
|
||||||
|
} while (0)
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::try_return_supervised(Tracker &tracker, const test_t *test, char *msg)
|
||||||
|
{
|
||||||
|
// Return along the shortest path while checking if it's what we expect
|
||||||
|
|
||||||
|
Tracker::path_finding_context_t context;
|
||||||
|
float x, y, z;
|
||||||
|
inner_assert(tracker.init_return_path(context, x, y, z), "tracker could not init return path");
|
||||||
|
return_state_t state = init_return_state(x, y, z, test);
|
||||||
|
|
||||||
|
for (;;) {
|
||||||
|
|
||||||
|
if (!tracker.advance_return_path(context, x, y, z)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t target = test->ret[state.target_index];
|
||||||
|
inner_assert(!tracker.get_graph_fault(), "graph inconsistent");
|
||||||
|
float accuracy = tracker.get_accuracy_at(x, y, z);
|
||||||
|
inner_assert(accuracy <= 5, "tracker gave an unsatisfying accuracy guarantee (%f)",
|
||||||
|
(double)accuracy); // arbitrarily chosen
|
||||||
|
inner_assert(advance_state(state, x, y, z, accuracy),
|
||||||
|
"vehicle diverted from expected return path: (%f, %f, %f) is not towards target %d %d %d", (double)x, (double)y,
|
||||||
|
(double)z, test->path[target * 3], test->path[target * 3 + 1], test->path[target * 3 + 2]);
|
||||||
|
inner_assert(state.no_progress < TRACKER_MAX_NO_PROGRESS,
|
||||||
|
"no progress detected from (%f, %f, %f) to (%f, %f, %f), target is %d %d %d", (double)state.x, (double)state.y,
|
||||||
|
(double)state.z, (double)x, (double)y, (double)z, test->path[target * 3], test->path[target * 3 + 1],
|
||||||
|
test->path[target * 3 + 2]);
|
||||||
|
|
||||||
|
#ifdef TRACKER_TEST_LOOKAHEAD
|
||||||
|
// Prefetch the return path starting at the current position
|
||||||
|
Tracker::path_finding_context_t inner_context;
|
||||||
|
float inner_x, inner_y, inner_z;
|
||||||
|
inner_assert(tracker.init_return_path(inner_context, inner_x, inner_y, inner_z), "tracker could not init return path");
|
||||||
|
return_state_t inner_state = init_return_state(inner_x, inner_y, inner_z, test);
|
||||||
|
|
||||||
|
while (tracker.advance_return_path(inner_context, inner_x, inner_y, inner_z)) {
|
||||||
|
inner_assert(!tracker.get_graph_fault(), "graph inconsistent");
|
||||||
|
accuracy = tracker.get_accuracy_at(inner_x, inner_y, inner_z);
|
||||||
|
inner_assert(accuracy <= 5, "tracker gave an unsatisfying accuracy guarantee (%f)",
|
||||||
|
(double)accuracy); // arbitrarily chosen
|
||||||
|
inner_assert(advance_state(inner_state, inner_x, inner_y, inner_z, accuracy),
|
||||||
|
"look-ahead return path diverted from expected return path");
|
||||||
|
inner_assert(inner_state.no_progress < TRACKER_MAX_NO_PROGRESS,
|
||||||
|
"no progress detected in look-ahead return path from (%f, %f, %f) to (%f, %f, %f), target index is %zu",
|
||||||
|
(double)inner_state.x, (double)inner_state.y, (double)inner_state.z, (double)inner_x, (double)inner_y, (double)inner_z,
|
||||||
|
inner_state.target_index);
|
||||||
|
}
|
||||||
|
|
||||||
|
inner_assert(detect_completion(inner_state),
|
||||||
|
"look-ahead return path did not reach home, ended at (%f, %f, %f), target index is %zu", (double)inner_state.x,
|
||||||
|
(double)inner_state.y, (double)inner_state.z, inner_state.target_index);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Follow the return path by one position
|
||||||
|
TRACKER_DBG("advance to (%.2f %.2f %.2f)", (double)x, (double)y, (double)z);
|
||||||
|
tracker.update(x, y, z);
|
||||||
|
}
|
||||||
|
|
||||||
|
inner_assert(!tracker.get_graph_fault(), "graph inconsistent");
|
||||||
|
inner_assert(detect_completion(state), "vehicle did not reach home, currently at (%f, %f, %f), home is (%d, %d, %d)",
|
||||||
|
(double)state.x, (double)state.y, (double)state.z, test->path[0], test->path[1], test->path[2]);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::try_return_unsupervised(Tracker &tracker, const test_t *test, float home_x, float home_y,
|
||||||
|
float home_z, float home_to_path_dist, char *msg)
|
||||||
|
{
|
||||||
|
int steps = 0;
|
||||||
|
float x, y, z;
|
||||||
|
Tracker::path_finding_context_t context;
|
||||||
|
|
||||||
|
// As long as we're not home, return along the proposed path
|
||||||
|
inner_assert(tracker.init_return_path(context, x, y, z), "tracker could not init return path");
|
||||||
|
|
||||||
|
while (tracker.advance_return_path(context, x, y, z)) {
|
||||||
|
tracker.update(x, y, z);
|
||||||
|
inner_assert(!tracker.get_graph_fault(), "graph inconsistent");
|
||||||
|
inner_assert(steps++ < 256, "return-to-home did take too many steps"); // make sure the loop terminates
|
||||||
|
TRACKER_DBG("return from %d, %d, %d, home is %.3f, %.3f, %.3f", (int)x, (int)y, (int)z, (double)home_x, (double)home_y,
|
||||||
|
(double)home_z);
|
||||||
|
}
|
||||||
|
|
||||||
|
inner_assert(!tracker.get_graph_fault(), "graph inconsistent");
|
||||||
|
|
||||||
|
// Check if we're actually home
|
||||||
|
inner_assert(get_distance(x, y, z, home_x, home_y,
|
||||||
|
home_z) <= 2 * Tracker::GRID_SIZE + home_to_path_dist, // accuracy at home is twice the grid size
|
||||||
|
"the vehicle didn't return home: it went to (%.2f %.2f %.2f) while home is at (%.2f %.2f %.2f)",
|
||||||
|
(double)x, (double)y, (double)z, (double)home_x, (double)home_y, (double)home_z);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::fly_and_return_test()
|
||||||
|
{
|
||||||
|
for (size_t t = 0; t < sizeof(test_cases) / sizeof(test_cases[0]); t++) {
|
||||||
|
const test_t *test = test_cases + t;
|
||||||
|
|
||||||
|
if (!test->ret_size) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
TRACKER_DBG("running fly-and-return on %s %s", test->name,
|
||||||
|
rewrite_graph ? "with graph rewriting" : "without graph rewriting");
|
||||||
|
|
||||||
|
_tracker.reset_graph();
|
||||||
|
_tracker.set_home(test->path[0], test->path[1], test->path[2]);
|
||||||
|
|
||||||
|
// Simulate flight along the specified path
|
||||||
|
for (size_t p = 0; p < test->path_size; p += 3) {
|
||||||
|
_tracker.update(test->path[p], test->path[p + 1], test->path[p + 2]);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef DEBUG_TRACKER
|
||||||
|
_tracker.dump_graph();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Rewrite graph to contain nothing but the return path
|
||||||
|
if (rewrite_graph) {
|
||||||
|
_tracker.rewrite_graph();
|
||||||
|
|
||||||
|
#ifdef DEBUG_TRACKER
|
||||||
|
_tracker.dump_graph();
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return home
|
||||||
|
char msg[1024];
|
||||||
|
|
||||||
|
if (!try_return_supervised(_tracker, test, msg)) {
|
||||||
|
ut_assert(msg, false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::fly_and_leave_return_path_test()
|
||||||
|
{
|
||||||
|
for (size_t t = 0; t < sizeof(test_cases) / sizeof(test_cases[0]); t++) {
|
||||||
|
const test_t *test = test_cases + t;
|
||||||
|
TRACKER_DBG("running fly-and-leave-return-path on %s", test->name);
|
||||||
|
|
||||||
|
_tracker.reset_graph();
|
||||||
|
_tracker.set_home(test->path[0], test->path[1], test->path[2]);
|
||||||
|
|
||||||
|
// Simulate flight along the specified path
|
||||||
|
for (size_t p = 0; p < test->path_size; p += 3) {
|
||||||
|
_tracker.update(test->path[p], test->path[p + 1], test->path[p + 2]);
|
||||||
|
}
|
||||||
|
|
||||||
|
int x = 0, y = 0, z = 0;
|
||||||
|
|
||||||
|
// Follow half of the return path
|
||||||
|
for (size_t r = 0; r < test->ret_size / 2; r++) {
|
||||||
|
x = test->path[test->ret[r] * 3];
|
||||||
|
y = test->path[test->ret[r] * 3 + 1];
|
||||||
|
z = test->path[test->ret[r] * 3 + 2];
|
||||||
|
_tracker.update(x, y, z);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Select a destination using a deterministic pseudorandom index
|
||||||
|
size_t dest_index = (x + y + z + test->ret[test->ret_size / 2]) % (test->path_size / 3);
|
||||||
|
int dest_x = test->path[dest_index * 3];
|
||||||
|
int dest_y = test->path[dest_index * 3 + 1];
|
||||||
|
int dest_z = test->path[dest_index * 3 + 2];
|
||||||
|
|
||||||
|
// Fly to the specified index (todo: when the tracker's delta limit gets fixed, we can directly jump to the destination).
|
||||||
|
while (x != dest_x || y != dest_y || z != dest_z) {
|
||||||
|
x += math::min(15, math::max(-16, dest_x - x));
|
||||||
|
y += math::min(15, math::max(-16, dest_y - y));
|
||||||
|
z += math::min(15, math::max(-16, dest_z - z));
|
||||||
|
_tracker.update(x, y, z);
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef DEBUG_TRACKER
|
||||||
|
_tracker.dump_graph();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Return home
|
||||||
|
char msg[512];
|
||||||
|
|
||||||
|
if (!try_return_unsupervised(_tracker, test, test->path[0], test->path[1], test->path[2], 0, msg)) {
|
||||||
|
ut_assert(msg, false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::fly_and_change_home_test()
|
||||||
|
{
|
||||||
|
for (size_t t = 0; t < sizeof(test_cases) / sizeof(test_cases[0]); t++) {
|
||||||
|
const test_t *test = test_cases + t;
|
||||||
|
TRACKER_DBG("running fly-and-change-home on %s", test->name);
|
||||||
|
|
||||||
|
_tracker.reset_graph();
|
||||||
|
_tracker.set_home(test->path[0], test->path[1], test->path[2]);
|
||||||
|
|
||||||
|
// Simulate flight along the specified path
|
||||||
|
for (size_t p = 0; p < test->path_size; p += 3) {
|
||||||
|
_tracker.update(test->path[p], test->path[p + 1], test->path[p + 2]);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Select a home using a deterministic pseudorandom index
|
||||||
|
size_t dest_index = (test->path[0] + test->path[1] + test->path[2] + test->ret[test->ret_size / 2]) %
|
||||||
|
(test->path_size / 3);
|
||||||
|
int home_x = test->path[dest_index * 3];
|
||||||
|
int home_y = test->path[dest_index * 3 + 1] + Tracker::GRID_SIZE; // add a little bit of offset
|
||||||
|
int home_z = test->path[dest_index * 3 + 2] + Tracker::GRID_SIZE; // add a little bit of offset
|
||||||
|
|
||||||
|
_tracker.set_home(home_x, home_y, home_z);
|
||||||
|
|
||||||
|
#ifdef DEBUG_TRACKER
|
||||||
|
_tracker.dump_graph();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Return home
|
||||||
|
char msg[512];
|
||||||
|
|
||||||
|
if (!try_return_unsupervised(_tracker, test, home_x, home_y, home_z, (double)sqrt(2) * (double)Tracker::GRID_SIZE,
|
||||||
|
msg)) {
|
||||||
|
ut_assert(msg, false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::performance_test()
|
||||||
|
{
|
||||||
|
for (size_t t = 0; t < sizeof(test_cases) / sizeof(test_cases[0]); t++) {
|
||||||
|
const test_t *test = test_cases + t;
|
||||||
|
PX4_WARN("running performance test on %s", test->name);
|
||||||
|
|
||||||
|
_tracker.reset_graph();
|
||||||
|
_tracker.set_home(test->path[0], test->path[1], test->path[2]);
|
||||||
|
|
||||||
|
// Simulate flight along the specified path
|
||||||
|
for (size_t p = 0; p < test->path_size; p += 3) {
|
||||||
|
_tracker.update(test->path[p], test->path[p + 1], test->path[p + 2]);
|
||||||
|
}
|
||||||
|
|
||||||
|
char number[128] = { 0 };
|
||||||
|
char msg1[256] = { 0 };
|
||||||
|
char msg2[256] = { 0 };
|
||||||
|
|
||||||
|
// print stats of each performance measurement
|
||||||
|
for (int i = 0; i < _tracker.memory_pressure - 1; i++) {
|
||||||
|
Tracker::compress_perf_t *perf = _tracker.perf_measurements + i;
|
||||||
|
|
||||||
|
sprintf(number, "%zu, %zu; %zu, %zu; ", perf->deltas_before, perf->nodes_before, perf->deltas_after, perf->nodes_after);
|
||||||
|
std::strcat(msg1, number);
|
||||||
|
|
||||||
|
sprintf(number, "%.3f ", (double)perf->runtime / (double)1e3f);
|
||||||
|
std::strcat(msg2, number);
|
||||||
|
}
|
||||||
|
|
||||||
|
// print final memory stats
|
||||||
|
sprintf(number, "%zu, %zu; 0, 0", _tracker.graph_next_write, _tracker.node_count);
|
||||||
|
std::strcat(msg1, number);
|
||||||
|
|
||||||
|
_tracker.dump_graph();
|
||||||
|
PX4_WARN("memory pressure: %d", _tracker.memory_pressure);
|
||||||
|
PX4_WARN("memory usage: [%s]", msg1);
|
||||||
|
PX4_WARN("CPU usage: [%s]", msg2);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::line_to_line_test()
|
||||||
|
{
|
||||||
|
size_t count = sizeof(line_test_cases) / sizeof(line_test_cases[0]);
|
||||||
|
|
||||||
|
for (size_t i = 0; i < 2 * count; i++) {
|
||||||
|
// We do each test twice, the second time we swap the lines
|
||||||
|
line_test_t *t = line_test_cases + (i % count);
|
||||||
|
|
||||||
|
if (i >= count) {
|
||||||
|
Tracker::ipos_t temp = t->delta1;
|
||||||
|
t->delta1 = t->delta2;
|
||||||
|
t->delta2 = temp;
|
||||||
|
temp = t->end1;
|
||||||
|
t->end1 = t->end2;
|
||||||
|
t->end2 = temp;
|
||||||
|
t->result = -t->result;
|
||||||
|
}
|
||||||
|
|
||||||
|
int coef1, coef2;
|
||||||
|
Tracker::ipos_t delta = Tracker::get_line_to_line_delta(t->delta1, t->end1, t->delta2, t->end2, coef1, coef2, false,
|
||||||
|
false);
|
||||||
|
|
||||||
|
Tracker::ipos_t p1 = t->end1 - Tracker::apply_coef(t->delta1, coef1);
|
||||||
|
Tracker::ipos_t p2 = t->end2 - Tracker::apply_coef(t->delta2, coef2);
|
||||||
|
Tracker::ipos_t implied_delta = p2 - p1;
|
||||||
|
|
||||||
|
char msg[512];
|
||||||
|
sprintf(msg,
|
||||||
|
"(%d %d %d) - %.2f * (%d %d %d) = (%d %d %d), (%d %d %d) - %.2f * (%d %d %d) = (%d %d %d), delta should be (%d %d %d) but have (%d %d %d)",
|
||||||
|
t->end1.x, t->end1.y, t->end1.z, (double)Tracker::coef_to_float(coef1), t->delta1.x, t->delta1.y, t->delta1.z, p1.x,
|
||||||
|
p1.y, p1.z,
|
||||||
|
t->end2.x, t->end2.y, t->end2.z, (double)Tracker::coef_to_float(coef2), t->delta2.x, t->delta2.y, t->delta2.z, p2.x,
|
||||||
|
p2.y, p2.z,
|
||||||
|
t->result.x, t->result.y, t->result.z, delta.x, delta.y, delta.z);
|
||||||
|
|
||||||
|
ut_compare(msg, implied_delta.x, delta.x);
|
||||||
|
ut_compare(msg, implied_delta.y, delta.y);
|
||||||
|
ut_compare(msg, implied_delta.z, delta.z);
|
||||||
|
ut_compare(msg, delta.x, t->result.x);
|
||||||
|
ut_compare(msg, delta.y, t->result.y);
|
||||||
|
ut_compare(msg, delta.z, t->result.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool TrackerTest::run_tests()
|
||||||
|
{
|
||||||
|
ut_run_test(line_to_line_test);
|
||||||
|
|
||||||
|
rewrite_graph = false;
|
||||||
|
ut_run_test(fly_and_return_test);
|
||||||
|
ut_run_test(fly_and_leave_return_path_test);
|
||||||
|
ut_run_test(fly_and_change_home_test);
|
||||||
|
|
||||||
|
rewrite_graph = true;
|
||||||
|
ut_run_test(fly_and_return_test);
|
||||||
|
ut_run_test(fly_and_leave_return_path_test);
|
||||||
|
ut_run_test(fly_and_change_home_test);
|
||||||
|
|
||||||
|
// These tests have nothing to do with correctness, but we can use them to asses performance of the tracker.
|
||||||
|
//ut_run_test(performance_test);
|
||||||
|
|
||||||
|
return (_tests_failed == 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
ut_declare_test(trackerTest, TrackerTest)
|
||||||
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
@ -147,3 +147,20 @@ PARAM_DEFINE_INT32(RTL_CONE_ANG, 0);
|
||||||
* @group Return To Land
|
* @group Return To Land
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
|
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
|
|
@ -0,0 +1,638 @@
|
||||||
|
/***************************************************************************
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file tracker.h
|
||||||
|
*
|
||||||
|
* Tracks the flight graph in a memory optimized format.
|
||||||
|
*
|
||||||
|
* The flight graph is stored as a series of delta elements at `GRID_SIZE`
|
||||||
|
* resolution and a list of nodes. Both lists share the same buffer `graph`.
|
||||||
|
* Each delta element (`delta_item_t`) 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 `consolidate_graph` is called, 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 (`rewrite_graph`). One of these actions
|
||||||
|
* is repeated at each subsequent fill-up.
|
||||||
|
*
|
||||||
|
* Path finding information is generated/refreshed on demand (`refresh_distances`)
|
||||||
|
* and stored in the nodes. During return-to-home, the best direction to home
|
||||||
|
* is continuously evaluated (`calc_return_path`) by using the information stored
|
||||||
|
* in the nodes.
|
||||||
|
*
|
||||||
|
* @author Samuel Sadok <samuel.sadok@bluewin.ch>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <limits.h>
|
||||||
|
#include <uORB/topics/home_position.h>
|
||||||
|
#include <uORB/topics/vehicle_local_position.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <lib/ecl/geo/geo.h>
|
||||||
|
|
||||||
|
// Enables verbose debug messages by the flight path tracker
|
||||||
|
//#define DEBUG_TRACKER
|
||||||
|
|
||||||
|
#ifdef DEBUG_TRACKER
|
||||||
|
#define TRACKER_DBG(...) PX4_INFO(__VA_ARGS__)
|
||||||
|
#define DUMP_GRAPH() dump_graph()
|
||||||
|
#else
|
||||||
|
#define TRACKER_DBG(...) (void)0
|
||||||
|
#define DUMP_GRAPH() (void)0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define GRAPH_ERR(...) do { \
|
||||||
|
/*PX4_ERR("Unexpected exception in flight path tracker: the flight graph may now be inconsistent must be reset.");*/ \
|
||||||
|
PX4_ERR(__VA_ARGS__); \
|
||||||
|
graph_fault = true; \
|
||||||
|
} while (0)
|
||||||
|
|
||||||
|
|
||||||
|
class Tracker
|
||||||
|
{
|
||||||
|
friend class TrackerTest;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
struct path_finding_context_t;
|
||||||
|
|
||||||
|
// Informs the tracker about a new home position.
|
||||||
|
void set_home(home_position_s *position) { set_home(position->x, position->y, position->z); }
|
||||||
|
|
||||||
|
// Informs the tracker about a new current vehicle position.
|
||||||
|
void update(vehicle_local_position_s *position);
|
||||||
|
|
||||||
|
// Returns true if a graph fault occurred.
|
||||||
|
void get_origin(struct map_projection_reference_s &ref_pos, float &ref_alt)
|
||||||
|
{
|
||||||
|
ref_pos = _ref_pos;
|
||||||
|
ref_alt = _ref_alt;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pops the last position from the recent path. Returns false if the path is empty.
|
||||||
|
bool pop_recent_path(double &lat, double &lon, float &alt);
|
||||||
|
|
||||||
|
// Enables or disables tracking of the recent path.
|
||||||
|
void set_recent_path_tracking_enabled(bool enabled) { recent_path_tracking_enabled = enabled; }
|
||||||
|
|
||||||
|
// Returns true if a graph fault occurred.
|
||||||
|
bool get_graph_fault() { return graph_fault; }
|
||||||
|
|
||||||
|
// Returns the guaranteed tracking precision at the specified position.
|
||||||
|
float get_accuracy_at(float x, float y, float z) { return 2 * GRID_SIZE * fast_sqrt(get_granularity_at(to_ipos({ .x = x, .y = y, .z = z })), false); }
|
||||||
|
|
||||||
|
// Forces graph consolidation
|
||||||
|
void consolidate_graph() { consolidate_graph("external request"); }
|
||||||
|
|
||||||
|
// Rewrites the graph such that it contains nothing but the shortest path to home.
|
||||||
|
void rewrite_graph();
|
||||||
|
|
||||||
|
// Deletes the flight graph.
|
||||||
|
void reset_graph();
|
||||||
|
|
||||||
|
// Initializes a path finding context starting at the current position.
|
||||||
|
// The context must be viewed as an opaque value that can for instance be passed to the advance_return_path function.
|
||||||
|
// Returns true on success.
|
||||||
|
bool init_return_path(path_finding_context_t &context, float &x, float &y, float &z);
|
||||||
|
|
||||||
|
// Same as `init_return_path`, but returns the global position
|
||||||
|
bool init_return_path_global(path_finding_context_t &context, double &lat, double &lon, float &alt);
|
||||||
|
|
||||||
|
// Fetches the next position on the shortest path back home (starting at, but excluding the position stored by the provided context).
|
||||||
|
// The context is updated accordingly.
|
||||||
|
// Returns true if the returned position is valid.
|
||||||
|
bool advance_return_path(path_finding_context_t &context, float &x, float &y, float &z);
|
||||||
|
|
||||||
|
// Same as `advance_return_path`, but returns the global position
|
||||||
|
bool advance_return_path_global(path_finding_context_t &context, double &lat, double &lon, float &alt);
|
||||||
|
|
||||||
|
// Checks if the provided context is currently close to the latest position that the tracker recorded.
|
||||||
|
bool is_context_close_to_head(path_finding_context_t &context);
|
||||||
|
|
||||||
|
// Checks if the provided context is currently close to the home position of the tracker.
|
||||||
|
bool is_context_close_to_home(path_finding_context_t &context);
|
||||||
|
|
||||||
|
// Checks if both of the provided contexts refer to the same position.
|
||||||
|
bool is_same_pos(path_finding_context_t &context1, path_finding_context_t &context2);
|
||||||
|
|
||||||
|
// Dumps the points in the recent path to the log output
|
||||||
|
void dump_recent_path();
|
||||||
|
|
||||||
|
// Dumps the content of the full flight graph to the log output
|
||||||
|
void dump_graph();
|
||||||
|
|
||||||
|
// Dumps the payload value of each node in the graph
|
||||||
|
void dump_nodes();
|
||||||
|
|
||||||
|
// Dumps the shortest path from the current position to the home position
|
||||||
|
void dump_path_to_home();
|
||||||
|
|
||||||
|
private:
|
||||||
|
/*** configuration ***/
|
||||||
|
|
||||||
|
// Tracking accuracy in meters
|
||||||
|
// todo: test GRID_SIZE values other than 1
|
||||||
|
static constexpr float GRID_SIZE = 1;
|
||||||
|
|
||||||
|
// Number of positions that are retained in the recent path buffer.
|
||||||
|
// This must be a multiple of 16 (?)
|
||||||
|
static constexpr int RECENT_PATH_LENGTH = 32;
|
||||||
|
|
||||||
|
// Number of small lines that can be stored in the full flight graph.
|
||||||
|
// The graph size in bytes is usually twice this number.
|
||||||
|
// The actual number of position depends on how how well the path can be optimized and how many nodes are stored.
|
||||||
|
static constexpr int GRAPH_LENGTH = 256;
|
||||||
|
|
||||||
|
// Scan range used in the consolidation algorithm.
|
||||||
|
// If the entire buffer is scanned, CPU usage may be too high.
|
||||||
|
// If two close lines are stored further appart than this range, their closeness will not be detected.
|
||||||
|
static constexpr int GRAPH_SEARCH_RANGE = GRAPH_LENGTH;
|
||||||
|
|
||||||
|
// The maximum number of positions that should be recorded until the new positions are consolidated.
|
||||||
|
// At the beginning and end of each consolidation pass, usually some optimization opportunities are missed.
|
||||||
|
// For this reason, a larger number is preferred here.
|
||||||
|
// The only drawback of a larger number should be a high workload once consolidation kicks in.
|
||||||
|
static constexpr int MAX_CONSOLIDATION_DEBT = 64;
|
||||||
|
|
||||||
|
// Controls the maximum distance from home where the graph is retained at maximum precision.
|
||||||
|
// Everything at distance 2^(HIGH_PRECISION_RANGE/2+1) or closer falls into this regime.
|
||||||
|
// Note that even within this range, precision will be reduced if necessary.
|
||||||
|
static constexpr int HIGH_PRECISION_RANGE = 10; // corresponds to 64m
|
||||||
|
|
||||||
|
// The size of the cache used by the graph rewrite algorithm.
|
||||||
|
// This must be at least FAR_DELTA_SIZE.
|
||||||
|
// A smaller number results in higher CPU load when a rewrite is invoked.
|
||||||
|
// A larger number leads to a higher stack, so be careful.
|
||||||
|
static constexpr int REWRITE_CACHE_SIZE = 9; // 3 * FAR_DELTA_SIZE
|
||||||
|
|
||||||
|
// Maximum number of graph compression performance measurements (this can be 0 unless you're analyzing the graph performance)
|
||||||
|
static constexpr int MAX_PERF_MEASUREMENTS = 10;
|
||||||
|
|
||||||
|
|
||||||
|
// Limitations and properties inherent to the graph representation (should not be changed)
|
||||||
|
static constexpr int COMPACT_DELTA_MIN = -16;
|
||||||
|
static constexpr int COMPACT_DELTA_MAX = 15;
|
||||||
|
static constexpr int FAR_DELTA_MIN = -16383; // 0xC001
|
||||||
|
static constexpr int FAR_DELTA_MAX = 16383; // 0x3FFF
|
||||||
|
static constexpr int FAR_DELTA_SIZE = 3;
|
||||||
|
static constexpr int MAX_INDEX = 0xFFFF;
|
||||||
|
static constexpr int MAX_COEFFICIENT = 0x7FFF;
|
||||||
|
static constexpr int MAX_DISTANCE = 0x3FFF;
|
||||||
|
static constexpr int OBSOLETE_DELTA = 0xC000;
|
||||||
|
|
||||||
|
#if GRAPH_LENGTH > MAX_INDEX
|
||||||
|
#error "GRAPH_LENGTH too large"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/*** internal type definitions ***/
|
||||||
|
|
||||||
|
typedef uint16_t delta_item_t;
|
||||||
|
|
||||||
|
// sizeof(node_t) should be 12 bytes (verify this!)
|
||||||
|
struct node_t {
|
||||||
|
uint16_t index1; // identifies the first line of the intersection
|
||||||
|
uint16_t index2; // identifies the second line of the intersection
|
||||||
|
unsigned int dirty : 1; // 1 if the node received a new distance value or needs a new value.
|
||||||
|
unsigned int coef1 :
|
||||||
|
15; // (coef1 / (MAX_COEFFICIENT + 1)) specifies where on line 1 the intersection lies. 0 if it is at the end, 1 if it is at the beginning.
|
||||||
|
unsigned int obsolete : 1; // set to 1 to mark this node for deletion
|
||||||
|
unsigned int coef2 :
|
||||||
|
15; // (coef2 / (MAX_COEFFICIENT + 1)) specifies where on line 2 the intersection lies. 0 if it is at the end, 1 if it is at the beginning.
|
||||||
|
|
||||||
|
delta_item_t
|
||||||
|
delta; // The intersection points on line1 and line2 are usually not precisely equal. This field specifies the difference between them.
|
||||||
|
|
||||||
|
// Note that an intersection is often not exact but consists of two distinct points.
|
||||||
|
// In this case, the distance of either point to home is upper bounded by the node distance.
|
||||||
|
// In some cases, both points are a bit closer than this upper bound.
|
||||||
|
|
||||||
|
// Currently, a node only stores routing information for one destination (home).
|
||||||
|
// Conceptually, we could support multiple destinations, by replicating the following fields for each destination.
|
||||||
|
// When the set of destinations changes, the node size would change, so we need to delete all nodes and regenerate them.
|
||||||
|
unsigned int use_line2 : 1; // true if the specified distance can be achieved by walking on line 2
|
||||||
|
unsigned int go_forward : 1; // true if the specified distance can be achieved by walking forward (NOT USED CURRENTLY)
|
||||||
|
unsigned int distance :
|
||||||
|
14; // Specifies the distance of this intersection to home. A value of MAX_DISTANCE means infinity.
|
||||||
|
};
|
||||||
|
|
||||||
|
struct fpos_t {
|
||||||
|
float x{0};
|
||||||
|
float y{0};
|
||||||
|
float z{0};
|
||||||
|
|
||||||
|
fpos_t operator*(const float scalar) const
|
||||||
|
{
|
||||||
|
return {
|
||||||
|
.x = this->x * scalar,
|
||||||
|
.y = this->y * scalar,
|
||||||
|
.z = this->z * scalar
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
fpos_t operator-(const fpos_t &pos2) const
|
||||||
|
{
|
||||||
|
return {
|
||||||
|
.x = this->x - pos2.x,
|
||||||
|
.y = this->y - pos2.y,
|
||||||
|
.z = this->z - pos2.z
|
||||||
|
};
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
struct ipos_t {
|
||||||
|
int x{0};
|
||||||
|
int y{0};
|
||||||
|
int z{0};
|
||||||
|
|
||||||
|
bool operator==(const ipos_t &pos2) const
|
||||||
|
{
|
||||||
|
return this->x == pos2.x && this->y == pos2.y && this->z == pos2.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
ipos_t operator+(const ipos_t &pos2) const
|
||||||
|
{
|
||||||
|
return {
|
||||||
|
.x = this->x + pos2.x,
|
||||||
|
.y = this->y + pos2.y,
|
||||||
|
.z = this->z + pos2.z
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
ipos_t operator-(const ipos_t &pos2) const
|
||||||
|
{
|
||||||
|
return {
|
||||||
|
.x = this->x - pos2.x,
|
||||||
|
.y = this->y - pos2.y,
|
||||||
|
.z = this->z - pos2.z
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
ipos_t operator+=(const ipos_t &pos2)
|
||||||
|
{
|
||||||
|
this->x += pos2.x;
|
||||||
|
this->y += pos2.y;
|
||||||
|
this->z += pos2.z;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
ipos_t operator-=(const ipos_t &pos2)
|
||||||
|
{
|
||||||
|
this->x -= pos2.x;
|
||||||
|
this->y -= pos2.y;
|
||||||
|
this->z -= pos2.z;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
ipos_t operator-() const
|
||||||
|
{
|
||||||
|
return {
|
||||||
|
.x = -this->x,
|
||||||
|
.y = -this->y,
|
||||||
|
.z = -this->z
|
||||||
|
};
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/*** utility functions ***/
|
||||||
|
static int round(float f)
|
||||||
|
{
|
||||||
|
return (int)(f + (f < 0 ? -0.5f : 0.5f));
|
||||||
|
};
|
||||||
|
|
||||||
|
static fpos_t to_fpos(ipos_t pos)
|
||||||
|
{
|
||||||
|
return { .x = (float)pos.x * GRID_SIZE, .y = (float)pos.y * GRID_SIZE, .z = (float)pos.z * GRID_SIZE };
|
||||||
|
}
|
||||||
|
|
||||||
|
static ipos_t to_ipos(fpos_t pos)
|
||||||
|
{
|
||||||
|
return { .x = round(pos.x / GRID_SIZE), .y = round(pos.y / GRID_SIZE), .z = round(pos.z / GRID_SIZE) };
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculates the square root of an integer, based on a look-up table.
|
||||||
|
// If the range of the look-up table is exceeded, the following action is taken:
|
||||||
|
// fallback_to_infinity: true: INFINITY is returned, false: the usual sqrt function is called
|
||||||
|
static float fast_sqrt(uint16_t val, bool fallback_to_infinity);
|
||||||
|
|
||||||
|
// Calculates the scalar product of two vectors. If any of the input vectors is very long (> ~16'000), this returns INT_MAX to prevent overflow.
|
||||||
|
static int dot(ipos_t vec1, ipos_t vec2);
|
||||||
|
|
||||||
|
// Calculates the squared length of a vector (the scalar product of the vector with itself).
|
||||||
|
static inline int dot(ipos_t vec) { return dot(vec, vec); }
|
||||||
|
|
||||||
|
// Determines the shortest from a point to a lines, as well as the point on the line that correspond to this distance.
|
||||||
|
static ipos_t get_point_to_line_delta(ipos_t point, ipos_t line_delta, ipos_t line_end, int &coef);
|
||||||
|
|
||||||
|
// Determines the shortest distance between two lines, as well as the points on the lines that correspond to this distance.
|
||||||
|
// Either of the two coefficients can be pinned, in which case a point-to-line or point-to-point delta is returned.
|
||||||
|
static ipos_t get_line_to_line_delta(ipos_t delta1, ipos_t end1, ipos_t delta2, ipos_t end2, int &coef1, int &coef2,
|
||||||
|
bool pin_coef1, bool pin_coef2);
|
||||||
|
|
||||||
|
// Converts a floating point number (on a scale of 0 to 1) to a discrete coefficient representation (on a scale of 0 to MAX_COEFFICIENT + 1)
|
||||||
|
static inline int float_to_coef(float coef) { return coef * (MAX_COEFFICIENT + 1); }
|
||||||
|
|
||||||
|
// Converts a discrete coefficient representation (on a scale of 0 to MAX_COEFFICIENT + 1) to a floating point number (on a scale of 0 to 1)
|
||||||
|
static inline float coef_to_float(int coef) { return coef / (float)(MAX_COEFFICIENT + 1); }
|
||||||
|
|
||||||
|
// Applies a coefficient to a delta and discretizes the result
|
||||||
|
static ipos_t apply_coef(ipos_t delta, int coef) { return to_ipos(to_fpos(delta) * coef_to_float(coef)); }
|
||||||
|
|
||||||
|
static inline delta_item_t pack_compact_delta_item(ipos_t delta);
|
||||||
|
static inline ipos_t unpack_compact_delta_item(delta_item_t delta);
|
||||||
|
|
||||||
|
// Returns true if the vector would be too large for a far delta element
|
||||||
|
static inline bool fits_into_far_delta(ipos_t vec);
|
||||||
|
|
||||||
|
// Pushes the specified delta item to a delta list (usually the graph buffer).
|
||||||
|
// If possible, the delta is stored as a compact delta item.
|
||||||
|
// Else, it is split into multiple items or stored as a far delta element, whichever is smaller.
|
||||||
|
// The caller must ensure that there are at least max_space free slots at the specified index.
|
||||||
|
// index:
|
||||||
|
// in: the delta will be stored starting at this index
|
||||||
|
// out: set to one index after the delta that was stored
|
||||||
|
// max_space: The maximum number of slots that may be used.
|
||||||
|
// Returns: true if the delta was pushed, false if the space was insufficient. If max_space >= FAR_DELTA_SIZE, the function always succeeds.
|
||||||
|
static bool push_delta(size_t &index, ipos_t delta, bool is_jump, ssize_t max_space, delta_item_t *buffer);
|
||||||
|
inline bool push_delta(size_t &index, ipos_t delta, bool is_jump, size_t max_space = FAR_DELTA_SIZE) { return push_delta(index, delta, is_jump, max_space, graph); };
|
||||||
|
|
||||||
|
// Fetches a delta item from the specified index.
|
||||||
|
// If the delta spans multiple indices, it is identified by its last index.
|
||||||
|
static ipos_t fetch_delta(size_t index, bool &is_jump, delta_item_t *buffer);
|
||||||
|
inline ipos_t fetch_delta(size_t index, bool &is_jump) { return fetch_delta(index, is_jump, graph); };
|
||||||
|
|
||||||
|
|
||||||
|
/*** private functions ***/
|
||||||
|
|
||||||
|
// Returns a granularity measure of the graph at the specified position.
|
||||||
|
// This is a function of the distance to home and memory pressure.
|
||||||
|
// The granularity is the square of half of the accuracy guarantee.
|
||||||
|
int get_granularity_at(ipos_t pos);
|
||||||
|
|
||||||
|
// Informs the tracker about a new home position.
|
||||||
|
void set_home(float x, float y, float z);
|
||||||
|
|
||||||
|
// Informs the tracker about a new current vehicle position.
|
||||||
|
void update(float x, float y, float z);
|
||||||
|
|
||||||
|
// Pushes a new current position to the recent path. This works even while the recent path is disabled.
|
||||||
|
void push_recent_path(fpos_t &position);
|
||||||
|
|
||||||
|
// Pops the last position from the recent path. Returns false if the path is empty.
|
||||||
|
bool pop_recent_path(fpos_t &position);
|
||||||
|
|
||||||
|
// Returns the size of the free area in the graph.
|
||||||
|
size_t get_free_graph_space() { return (GRAPH_LENGTH - graph_next_write) * sizeof(delta_item_t) - node_count * sizeof(node_t); };
|
||||||
|
|
||||||
|
// Pushes a new current position to the flight graph.
|
||||||
|
void push_graph(fpos_t &position);
|
||||||
|
|
||||||
|
// Reads one delta element from the graph in the forward direction.
|
||||||
|
// The index must point to the end of a delta element and is pre-incremented.
|
||||||
|
ipos_t walk_forward(size_t &index, bool &is_jump);
|
||||||
|
|
||||||
|
// Reads one delta element from the graph in the backward direction.
|
||||||
|
// The index must point to the end of a delta element and is post-decremented.
|
||||||
|
ipos_t walk_backward(size_t &index, bool &is_jump);
|
||||||
|
|
||||||
|
// Returns the node at the specified index.
|
||||||
|
node_t &node_at(size_t index) { return *(nodes - index); };
|
||||||
|
|
||||||
|
// Pushes a node to the node stack if there is enough space.
|
||||||
|
// Returns true if the operation succeeds.
|
||||||
|
// Returns false if there is not enough space of if there is already a nearby node.
|
||||||
|
bool push_node(node_t &node, int granularity);
|
||||||
|
|
||||||
|
// Removes all nodes (except the home node) that reference lines within the specified range.
|
||||||
|
// The lower bound is exclusive, the upper bound is inclusive.
|
||||||
|
void remove_nodes(size_t lower_bound, size_t upper_bound);
|
||||||
|
|
||||||
|
// Checks if two positions are similar.
|
||||||
|
// This means there's at most one vertex between them, there's no jump between them and their distance along the line is not too large.
|
||||||
|
// Returns true if these conditions are satisfied.
|
||||||
|
bool check_similarity(size_t index1, int coef1, size_t index2, int coef2, float max_distance);
|
||||||
|
|
||||||
|
// Returns true if the specified position is close to any line in the graph.
|
||||||
|
// lower_bound: exclusive lower search bound
|
||||||
|
// upper_bound: inclusive upper search bound
|
||||||
|
bool is_close_to_graph(ipos_t position, size_t lower_bound, size_t upper_bound, ipos_t pos_at_bound);
|
||||||
|
|
||||||
|
// Searches the entire graph for the position which is closest to the specified one.
|
||||||
|
// best_index, best_coef: specifies the index and coefficient of the closest position on the graph. Both are 0 if the graph is empty.
|
||||||
|
ipos_t get_closest_position(ipos_t position, size_t &best_index, unsigned int &best_coef);
|
||||||
|
|
||||||
|
// Determines if all points between start and end lie on a straight line.
|
||||||
|
// The points are allowed to have the distance max_deviation to the line.
|
||||||
|
bool is_line(ipos_t start_pos, size_t start_index, ipos_t end_pos, size_t end_index, bool should_be_jump);
|
||||||
|
|
||||||
|
// Returns the latest index to which a straight line can be drawn from start_index
|
||||||
|
// start_pos: the position corresponding to start_index
|
||||||
|
// start_index: the index at which the line should start
|
||||||
|
// end_pos:
|
||||||
|
// in: ignored
|
||||||
|
// out: the position corresponding to end_index
|
||||||
|
// end_index:
|
||||||
|
// in: ignored
|
||||||
|
// out: the index at which the longest possible line ends (start_index < end_index <= bound)
|
||||||
|
// is_jump: set to the jump property of the line under consideration (jump and non-jump deltas are never aggregated)
|
||||||
|
// bound: the line is constrained not to go beyond this index, but it may end there.
|
||||||
|
void get_longest_line(ipos_t start_pos, size_t start_index, ipos_t &end_pos, size_t &end_index, bool &is_jump,
|
||||||
|
size_t bound);
|
||||||
|
|
||||||
|
// Optimizes memory usage of the most recent positions in the graph.
|
||||||
|
void consolidate_graph(const char *reason);
|
||||||
|
|
||||||
|
// Makes sure that the meaning of the specified index does not change unless the graph version is incremented.
|
||||||
|
size_t pin_index(size_t index) { pinned_index = index > pinned_index ? index : pinned_index; return index; }
|
||||||
|
|
||||||
|
// Measures the direct distance between two positions on the graph.
|
||||||
|
// Returns infinity if the positions are separated by a jump.
|
||||||
|
float measure_distance(size_t index1, int coef1, size_t index2, int coef2);
|
||||||
|
|
||||||
|
// Walks the graph from the specified position up to the next node, while tracking the covered distance.
|
||||||
|
// If the search bound or a jump is encountered before a node, the function returns false.
|
||||||
|
// Note that jumps can have nodes as well, but only at the end. Therefore, walking backwards may end at a jump node and walking forward may start at one.
|
||||||
|
// index, coef:
|
||||||
|
// in: The index/coefficient at which to start the search.
|
||||||
|
// out: The index/coefficient at which a node was encountered (invalid if the function returns false).
|
||||||
|
// distance: The distance that was covered from the start up to the node (invalid if the function returns false).
|
||||||
|
// forward: true if the function should walk forward, false if it should walk backward.
|
||||||
|
// search_bound: bounds the search (inclusive in both directions)
|
||||||
|
bool walk_to_node(size_t &index, int &coef, float &distance, bool forward, size_t search_bound);
|
||||||
|
|
||||||
|
// Uses the best node at the specified position to switch to the line which represents the shortest path home.
|
||||||
|
// The distance to home is also returned.
|
||||||
|
// index, coef:
|
||||||
|
// in: the position at which the nodes should be examined
|
||||||
|
// out: the position that leads home
|
||||||
|
// delta: if not nullptr, set to the position difference between the input and output positions
|
||||||
|
// go_forward: set to the best direction to home from the new line (valid iif the position was at a node, i.e. if the result is finite)
|
||||||
|
float apply_node_delta(size_t &index, unsigned int &coef, ipos_t *delta, bool &go_forward);
|
||||||
|
|
||||||
|
// Returns the distance to home of any position on the graph which coincides with a node,
|
||||||
|
// including an indication on how this distance can be achieved.
|
||||||
|
// index, coef: the position for which the distance-to-home should be retrieved
|
||||||
|
// direction: 0: switch line, 1: go forward from here, -1: go backward from here
|
||||||
|
float get_node_distance(size_t index, unsigned int coef, int &direction);
|
||||||
|
|
||||||
|
// Sets the distance to home of any position on the graph which corresponds to one or multiple nodes.
|
||||||
|
// The function takes only improving action (i.e. only reduces distances) and returns true if did so.
|
||||||
|
// go_forward: indicates if the distance can be achieved by walking forward
|
||||||
|
bool set_node_distance(size_t index, int coef, float distance, bool go_forward);
|
||||||
|
|
||||||
|
// Ensures that each node has a valid distance-to-home value.
|
||||||
|
void refresh_distances(void);
|
||||||
|
|
||||||
|
// Calculates the distance to home from a specific index in both forward and backward directions.
|
||||||
|
//void get_distance_to_home(size_t index, float bias, float &dist_forward, float &dist_backward);
|
||||||
|
|
||||||
|
// Returns the index (and bias) which is closest to the specified position.
|
||||||
|
//size_t get_closest_index(ipos_t position, float *bias);
|
||||||
|
|
||||||
|
// Calculates the next best move to get back home, using the provided path finding context.
|
||||||
|
bool calc_return_path(path_finding_context_t &context, bool &progress);
|
||||||
|
|
||||||
|
// Marks all delta elements in the forward or backward direction obsolete, until a node is encountered.
|
||||||
|
// index, coef: where to start marking deltas as obsolete. The starting index is always kept.
|
||||||
|
// forward, backward: which direction to go to mark deltas as obsolete
|
||||||
|
// retain: if this item is reached, the walk is aborted
|
||||||
|
// lower_bound, upper_bound: the exclusive bounds of the walk
|
||||||
|
void mark_obsolete(size_t index, int coef, bool forward, bool backward, size_t retain, size_t lower_bound,
|
||||||
|
size_t upper_bound);
|
||||||
|
|
||||||
|
|
||||||
|
/*** internal variables ***/
|
||||||
|
|
||||||
|
struct map_projection_reference_s _ref_pos {};
|
||||||
|
float _ref_alt{0.f};
|
||||||
|
uint64_t _ref_timestamp{0};
|
||||||
|
|
||||||
|
ipos_t home_position{};
|
||||||
|
ipos_t home_on_graph{};
|
||||||
|
bool did_set_home{false};
|
||||||
|
|
||||||
|
bool recent_path_tracking_enabled{true};
|
||||||
|
bool graph_tracking_enabled{true};
|
||||||
|
|
||||||
|
fpos_t last_known_position{};
|
||||||
|
|
||||||
|
// The most recent position in the recent path. The absolute positions in the path can be calculated based on this.
|
||||||
|
// If tracking has not yet started (that is, if the path is empty), this is invalid.
|
||||||
|
ipos_t recent_path_head{};
|
||||||
|
|
||||||
|
// Stores the (potentially shortened) recent flight path as a ring buffer.
|
||||||
|
// The recent path respects the following invariant: No two points are closer than the grid size.
|
||||||
|
// This buffer contains only delta items. Each item stores a position relative to the previous position in the path.
|
||||||
|
// The very first item is a bumper that indicates that the initial position (0,0,0) is not valid. This bumper will disappear once the ring buffer overflows.
|
||||||
|
delta_item_t recent_path[RECENT_PATH_LENGTH] = { 0xFFFF };
|
||||||
|
|
||||||
|
size_t recent_path_next_write{1}; // always valid, 0 if empty, equal to next_read if full
|
||||||
|
size_t recent_path_next_read{0}; // RECENT_PATH_LENGTH if empty, valid if non-empty
|
||||||
|
|
||||||
|
// Indicates that the graph may have become inconsistent.
|
||||||
|
// If this occurs, it must be reset at the next opportunity.
|
||||||
|
bool graph_fault{false};
|
||||||
|
|
||||||
|
// Keeps track of the memory pressure to allow for adaptive graph precision.
|
||||||
|
// Each time the graph becomes full, the memory pressure is incremented by one.
|
||||||
|
// Each even time, the graph precision is reduced, each odd time, the graph is rewritten.
|
||||||
|
int memory_pressure{1};
|
||||||
|
|
||||||
|
// The first valid position in the graph, corresponding to the first position in the buffer.
|
||||||
|
ipos_t graph_start{};
|
||||||
|
|
||||||
|
// The last position in the graph, corresponding to the end of the buffer.
|
||||||
|
// This roughly corresponds to the current vehicle position.
|
||||||
|
// The absolute positions in the path can be calculated based on this.
|
||||||
|
ipos_t graph_head_pos{};
|
||||||
|
|
||||||
|
// The next free index in the graph.
|
||||||
|
size_t graph_next_write{0};
|
||||||
|
|
||||||
|
// Stores the entire flight path (until the buffer is full).
|
||||||
|
// Each delta item represents a line from the previous position to a new position.
|
||||||
|
// Sometimes (if the vehicle moves along an already visited path), jump deltas are inserted.
|
||||||
|
// Note that the first item carries no valid information other than that the graph is non-empty.
|
||||||
|
delta_item_t graph[GRAPH_LENGTH];
|
||||||
|
|
||||||
|
// The consolidated head position represents the last position which shall no longer be considered by the consolidation pass.
|
||||||
|
// Moving this around arbitrarily should not result in undefined behaviour.
|
||||||
|
ipos_t consolidated_head_pos{};
|
||||||
|
size_t consolidated_head_index{0};
|
||||||
|
|
||||||
|
// If the graph is altered in a way that previously exported indices become invalid, the graph version is incremented.
|
||||||
|
int graph_version{0};
|
||||||
|
|
||||||
|
// If this index or anything before changes, the graph version must be incremented.
|
||||||
|
size_t pinned_index{0};
|
||||||
|
|
||||||
|
// Nodes keep track of lines in the flight path that pass close to each other.
|
||||||
|
// The nodes are stored at the end of the graph buffer, the node with the index 0 denotes the last node.
|
||||||
|
// When deltas and nodes collide, the graph is full.
|
||||||
|
// Node 0 (index1, coef1) represents the home position and must not be removed.
|
||||||
|
node_t *nodes = (node_t *)(graph + GRAPH_LENGTH) - 1;
|
||||||
|
size_t node_count{1};
|
||||||
|
|
||||||
|
// True as long as any nodes in the buffer have dirty set to 1
|
||||||
|
bool have_dirty_nodes{true};
|
||||||
|
|
||||||
|
struct compress_perf_t {
|
||||||
|
hrt_abstime runtime;
|
||||||
|
size_t deltas_before;
|
||||||
|
size_t nodes_before;
|
||||||
|
size_t deltas_after;
|
||||||
|
size_t nodes_after;
|
||||||
|
};
|
||||||
|
|
||||||
|
compress_perf_t perf_measurements[MAX_PERF_MEASUREMENTS];
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Tracker::path_finding_context_t {
|
||||||
|
ipos_t current_pos; // The position corresponding to (current_index, current_coef)
|
||||||
|
size_t current_index;
|
||||||
|
unsigned int current_coef;
|
||||||
|
size_t checkpoint_index; // If (current_index, current_coef) becomes equal to this checkpoint, a re-evaluation of the best direction is forced, which may involve switching lines on an intersection.
|
||||||
|
unsigned int checkpoint_coef;
|
||||||
|
|
||||||
|
int graph_version;
|
||||||
|
};
|
||||||
|
|
@ -122,6 +122,7 @@ const struct {
|
||||||
|
|
||||||
/* external tests */
|
/* external tests */
|
||||||
{"commander", commander_tests_main, 0},
|
{"commander", commander_tests_main, 0},
|
||||||
|
{"navigator", navigator_tests_main, 0},
|
||||||
{"controllib", controllib_test_main, 0},
|
{"controllib", controllib_test_main, 0},
|
||||||
{"mavlink", mavlink_tests_main, 0},
|
{"mavlink", mavlink_tests_main, 0},
|
||||||
#ifdef __PX4_NUTTX
|
#ifdef __PX4_NUTTX
|
||||||
|
|
|
||||||
|
|
@ -92,6 +92,7 @@ extern int test_versioning(int argc, char *argv[]);
|
||||||
|
|
||||||
/* external */
|
/* external */
|
||||||
extern int commander_tests_main(int argc, char *argv[]);
|
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 mavlink_tests_main(int argc, char *argv[]);
|
||||||
extern int controllib_test_main(int argc, char *argv[]);
|
extern int controllib_test_main(int argc, char *argv[]);
|
||||||
extern int uorb_tests_main(int argc, char *argv[]);
|
extern int uorb_tests_main(int argc, char *argv[]);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue