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.

Tools/Matlab/lineToLineTest.m

@@ -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

Tools/draw_path.sh

@@ -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
+

platforms/posix/cmake/sitl_tests.cmake

@@ -27,6 +27,7 @@ set(tests
 	microbench_matrix
 	microbench_uorb
 	mixer
+	navigator
 	param
 	parameters
 	perf

src/drivers/drv_hrt.h

@@ -57,6 +57,8 @@ __BEGIN_DECLS
  */
 typedef uint64_t	hrt_abstime;
 
+#define HRT_ABSTIME_MAX UINT64_MAX
+
 /**
  * Callout function type.
  *

src/modules/navigator/CMakeLists.txt

@@ -34,6 +34,8 @@
 px4_add_module(
 	MODULE modules__navigator
 	MAIN navigator
+	COMPILE_FLAGS
+		-Wno-error=cast-align
 	SRCS
 		navigator_main.cpp
 		navigator_mode.cpp
@@ -49,8 +51,14 @@ px4_add_module(
 		enginefailure.cpp
 		gpsfailure.cpp
 		follow_target.cpp
+		smart_rtl.cpp
+		tracker.cpp
 	DEPENDS
 		git_ecl
 		ecl_geo
 		landing_slope
 	)
+
+if(PX4_TESTING)
+	add_subdirectory(navigator_tests)
+endif()

src/modules/navigator/navigator.h

@@ -1,6 +1,6 @@
 /***************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -51,7 +51,9 @@
 #include "mission.h"
 #include "navigator_mode.h"
 #include "rtl.h"
+#include "smart_rtl.h"
 #include "takeoff.h"
+#include "tracker.h"
 
 #include "navigation.h"
 
@@ -81,7 +83,7 @@
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 9
+#define NAVIGATOR_MODE_ARRAY_SIZE 10
 
 
 class Navigator : public ModuleBase<Navigator>, public ModuleParams
@@ -169,6 +171,11 @@ public:
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_nav_loiter_rad.get(); }
 
+	Tracker		*get_tracker() { return &_tracker; }
+
+	// Quick and dirty, to do it cleanly, we may want to introduce a new navigator mode
+	void		set_rtl_variant(bool advanced) { _use_advanced_rtl = advanced; }
+
 	/**
 	 * Returns the default acceptance radius defined by the parameter
 	 */
@@ -285,6 +292,12 @@ public:
 
 	bool		abort_landing();
 
+	// Advanced RTL
+	void		tracker_reset() { _tracker.reset_graph(); }
+	void		tracker_consolidate() { _tracker.consolidate_graph(); }
+	void		tracker_rewrite() { _tracker.rewrite_graph(); }
+
+
 	// Param access
 	float		get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
 	float		get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
@@ -368,21 +381,25 @@ private:
 	Geofence	_geofence;			/**< class that handles the geofence */
 	bool		_geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
 
+	Tracker		_tracker;			/**< class that tracks the vehicle path for smart RTL **/
+
 	bool		_can_loiter_at_sp{false};			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated{false};		/**< flags if position SP triplet needs to be published */
 	bool 		_pos_sp_triplet_published_invalid_once{false};	/**< flags if position SP triplet has been published once to UORB */
 	bool		_mission_result_updated{false};		/**< flags if mission result has seen an update */
+	bool		_use_advanced_rtl{true};		/**< use graph-based RTL instead of direct RTL **/
 
-	NavigatorMode	*_navigation_mode{nullptr};		/**< abstract pointer to current navigation mode class */
+	NavigatorMode	*_navigation_mode{nullptr};	/**< abstract pointer to current navigation mode class */
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	Takeoff		_takeoff;			/**< class for handling takeoff commands */
-	Land		_land;			/**< class for handling land commands */
+	Land		_land;				/**< class for handling land commands */
 	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
 	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode (FW only!) */
 	GpsFailure	_gpsFailure;			/**< class that handles the OBC gpsfailure loss mode */
 	FollowTarget	_follow_target;
+	SmartRTL	_smartRtl;			/**< class that handles return-to-land along recorded flight graph */
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 

src/modules/navigator/navigator_main.cpp

@@ -86,7 +86,8 @@ Navigator::Navigator() :
 	_rtl(this),
 	_engineFailure(this),
 	_gpsFailure(this),
-	_follow_target(this)
+	_follow_target(this),
+	_smartRtl(this)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -98,6 +99,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[6] = &_land;
 	_navigation_mode_array[7] = &_precland;
 	_navigation_mode_array[8] = &_follow_target;
+	_navigation_mode_array[9] = &_smartRtl;
 
 	_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
 	_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -176,7 +178,18 @@ Navigator::run()
 		} else {
 			if (fds[0].revents & POLLIN) {
 				/* success, local pos is available */
-				orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+				if (orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos) == PX4_OK) {
+					if (!_land_detected.landed) {
+						if (_tracker.get_graph_fault()) {
+							_tracker.reset_graph();
+						}
+
+						_tracker.update(&_local_pos);
+
+					} else {
+						_use_advanced_rtl = true; // Try advanced RTL again for the next flight
+					}
+				}
 			}
 		}
 
@@ -212,9 +225,21 @@ Navigator::run()
 			params_update();
 		}
 
-		_land_detected_sub.update(&_land_detected);
+		if (_land_detected_sub.update(&_land_detected)) {
+			if (!_land_detected.landed) {
+				if (_tracker.get_graph_fault()) {
+					_tracker.reset_graph();
+				}
+
+				_tracker.update(&_local_pos);
+			}
+		}
+
 		_position_controller_status_sub.update();
-		_home_pos_sub.update(&_home_pos);
+
+		if (_home_pos_sub.update(&_home_pos)) {
+			_tracker.set_home(&_home_pos);
+		}
 
 		if (_vehicle_command_sub.updated()) {
 			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
@@ -632,11 +657,17 @@ Navigator::run()
 					break;
 
 				default:
-					if (rtl_activated) {
-						mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
+					if (_use_advanced_rtl) {
+						navigation_mode_new = &_smartRtl;
+
+					} else {
+						if (rtl_activated) {
+							mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
+						}
+
+						navigation_mode_new = &_rtl;
 					}
 
-					navigation_mode_new = &_rtl;
 					break;
 
 				}
@@ -772,6 +803,11 @@ Navigator::print_status()
 	PX4_INFO("Running");
 
 	_geofence.printStatus();
+
+	_tracker.dump_recent_path();
+	_tracker.dump_graph();
+	//_tracker.dump_path_to_home();
+
 	return 0;
 }
 
@@ -1219,6 +1255,25 @@ int Navigator::custom_command(int argc, char *argv[])
 					     transponder_report_s::ADSB_EMITTER_TYPE_LARGE);
 		get_instance()->fake_traffic("UAV", 10, 1.0f, -2.0f, 10.0f, 10.0f, 0.01f, transponder_report_s::ADSB_EMITTER_TYPE_UAV);
 		return 0;
+
+	} else if (!strcmp(argv[0], "tracker") && argc >= 2) {
+		if (!strcmp(argv[1], "reset")) {
+			// Deletes the entire flight graph (but not the most recent path!).
+			// This may be neccessary if the environment changed heavily since system start.
+			get_instance()->tracker_reset();
+
+		} else if (!strcmp(argv[1], "consolidate")) {
+			// Consolidates the flight graph.
+			// This is not required for normal operation, as it happens automatically.
+			get_instance()->tracker_consolidate();
+
+		} else if (!strcmp(argv[1], "rewrite")) {
+			// Deletes everything from the flight graph, which does not lead home.
+			// This is not required for normal operation, as it happens automatically.
+			get_instance()->tracker_rewrite();
+		}
+
+		return 0;
 	}
 
 	return print_usage("unknown command");

src/modules/navigator/navigator_tests/.clang-tidy

@@ -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'

src/modules/navigator/navigator_tests/CMakeLists.txt

@@ -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
+
+)

src/modules/navigator/navigator_tests/complex_loop.path

@@ -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,

src/modules/navigator/navigator_tests/from_sim.path

@@ -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,

src/modules/navigator/navigator_tests/large_nodes.path

@@ -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,

src/modules/navigator/navigator_tests/navigator_tests.cpp

@@ -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;
+}

src/modules/navigator/navigator_tests/no_loop.path

@@ -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,

src/modules/navigator/navigator_tests/simple_jumps.path

@@ -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,

src/modules/navigator/navigator_tests/simple_loop.path

@@ -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,

src/modules/navigator/navigator_tests/tracker_test.cpp

@@ -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)

src/modules/navigator/navigator_tests/tracker_test.h

@@ -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();

src/modules/navigator/rtl_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -147,3 +147,20 @@ PARAM_DEFINE_INT32(RTL_CONE_ANG, 0);
  * @group Return To Land
  */
 PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
+
+/**
+ * RTL Advanced to RTL classic fallback delay
+ *
+ * If the vehicle makes no progress in advanced RTL for this timespan for whatever reason, the system falls back to basic RTL mode.
+ * Note that there may be long straight lines in the return path. This delay should be large enough to allow flying along such lines.
+ * If set to -1 the system will not switch to basic RTL but loiter.
+ *
+ * @unit s
+ * @min -1
+ * @max 300
+ * @decimal 1
+ * @increment 0.5
+ * @group Return To Land
+ */
+PARAM_DEFINE_FLOAT(RTL_FALLBCK_DLY, 120f);
+

src/modules/navigator/smart_rtl.cpp

@@ -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);
+}

src/modules/navigator/smart_rtl.h

@@ -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
+	)
+};

src/modules/navigator/tracker.h

@@ -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;
+};

src/systemcmds/tests/tests_main.c

@@ -122,6 +122,7 @@ const struct {
 
 	/* external tests */
 	{"commander",		commander_tests_main,	0},
+	{"navigator",		navigator_tests_main,	0},
 	{"controllib",		controllib_test_main,	0},
 	{"mavlink",		mavlink_tests_main,	0},
 #ifdef __PX4_NUTTX

src/systemcmds/tests/tests_main.h

@@ -92,6 +92,7 @@ extern int test_versioning(int argc, char *argv[]);
 
 /* external */
 extern int commander_tests_main(int argc, char *argv[]);
+extern int navigator_tests_main(int argc, char *argv[]);
 extern int mavlink_tests_main(int argc, char *argv[]);
 extern int controllib_test_main(int argc, char *argv[]);
 extern int uorb_tests_main(int argc, char *argv[]);