diff --git a/Tools/Matlab/lineToLineTest.m b/Tools/Matlab/lineToLineTest.m new file mode 100644 index 0000000000..93b27d3daf --- /dev/null +++ b/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 diff --git a/Tools/draw_path.sh b/Tools/draw_path.sh new file mode 100755 index 0000000000..b55e2d1819 --- /dev/null +++ b/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 + diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index cf672dc8aa..00d03161be 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -27,6 +27,7 @@ set(tests microbench_matrix microbench_uorb mixer + navigator param parameters perf diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 1bfe6c7127..e25b5ca166 100644 --- a/src/drivers/drv_hrt.h +++ b/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. * diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 8298bba7dd..f9dbc26856 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/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() diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a92e131b04..4887a0fa1a 100644 --- a/src/modules/navigator/navigator.h +++ b/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, 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 */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 142f732c80..d6a2366ae6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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"); diff --git a/src/modules/navigator/navigator_tests/.clang-tidy b/src/modules/navigator/navigator_tests/.clang-tidy new file mode 100644 index 0000000000..b9d1a84f8c --- /dev/null +++ b/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' diff --git a/src/modules/navigator/navigator_tests/CMakeLists.txt b/src/modules/navigator/navigator_tests/CMakeLists.txt new file mode 100644 index 0000000000..54bc0f4429 --- /dev/null +++ b/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 + +) diff --git a/src/modules/navigator/navigator_tests/complex_loop.path b/src/modules/navigator/navigator_tests/complex_loop.path new file mode 100644 index 0000000000..14b49409bb --- /dev/null +++ b/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, diff --git a/src/modules/navigator/navigator_tests/from_sim.path b/src/modules/navigator/navigator_tests/from_sim.path new file mode 100644 index 0000000000..acf9b3e456 --- /dev/null +++ b/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, diff --git a/src/modules/navigator/navigator_tests/large_nodes.path b/src/modules/navigator/navigator_tests/large_nodes.path new file mode 100644 index 0000000000..17650da32d --- /dev/null +++ b/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, diff --git a/src/modules/navigator/navigator_tests/long_path1.path b/src/modules/navigator/navigator_tests/long_path1.path new file mode 100644 index 0000000000..9b287b82fc --- /dev/null +++ b/src/modules/navigator/navigator_tests/long_path1.path @@ -0,0 +1,1078 @@ +0,0,0, +0,0,-2, +0,0,-4, +0,0,-6, +0,0,-8, +0,0,-10, +0,0,-12, +0,0,-14, +0,0,-16, +0,0,-18, +0,0,-20, +0,0,-22, +0,0,-24, +2,0,-24, +3,0,-25, +5,0,-25, +7,0,-25, +8,-1,-25, +10,-1,-25, +12,-1,-25, +14,-1,-25, +16,-1,-25, +18,-1,-25, +19,-2,-25, +21,-2,-25, +23,-2,-25, +25,-2,-25, +26,-1,-25, +28,-1,-25, +29,0,-25, +30,1,-25, +31,2,-25, +32,3,-25, +33,4,-25, +35,4,-25, +36,5,-25, +37,6,-25, +38,7,-25, +40,7,-25, +41,8,-25, +43,8,-25, +44,9,-25, +46,9,-25, +47,10,-25, +48,11,-25, +50,11,-25, +51,12,-25, +52,13,-25, +54,13,-25, +55,14,-25, +57,14,-25, +58,15,-25, +60,15,-25, +61,16,-25, +63,16,-25, +64,17,-25, +66,17,-25, +67,18,-25, +69,18,-25, +70,19,-25, +72,19,-25, +73,20,-25, +74,21,-25, +76,21,-25, +77,22,-25, +79,22,-25, +80,23,-25, +82,23,-25, +83,24,-25, +85,24,-25, +86,25,-25, +88,25,-25, +89,26,-25, +91,26,-25, +92,27,-25, +94,27,-25, +95,28,-25, +92,27,-25, +91,26,-25, +89,26,-25, +88,25,-25, +87,24,-25, +85,24,-25, +84,23,-25, +82,23,-25, +81,22,-25, +79,22,-25, +78,21,-25, +76,21,-25, +75,20,-25, +74,19,-25, +72,19,-25, +71,18,-25, +69,18,-25, +68,17,-25, +67,16,-25, +65,16,-25, +64,15,-25, +62,15,-25, +61,14,-25, +60,13,-25, +58,13,-25, +57,12,-25, +55,12,-25, +54,11,-25, +52,11,-25, +51,10,-25, +50,9,-25, +48,9,-25, +47,8,-25, +45,8,-25, +44,7,-25, +43,6,-25, +41,6,-25, +40,5,-25, +38,5,-25, +37,4,-25, +35,4,-25, +34,3,-25, +33,2,-25, +31,2,-25, +30,1,-25, +29,0,-25, +29,-2,-25, +30,-3,-25, +31,-4,-25, +31,-6,-25, +32,-7,-25, +33,-8,-25, +33,-10,-25, +34,-11,-25, +34,-13,-25, +34,-15,-25, +35,-16,-25, +35,-18,-25, +35,-20,-25, +35,-22,-25, +36,-23,-25, +36,-25,-25, +35,-26,-25, +34,-27,-25, +32,-27,-25, +30,-27,-25, +29,-28,-25, +27,-28,-25, +25,-28,-25, +24,-29,-25, +23,-30,-25, +21,-30,-25, +20,-31,-25, +19,-32,-25, +17,-32,-25, +16,-33,-25, +15,-34,-25, +13,-34,-25, +12,-35,-25, +10,-35,-25, +9,-36,-25, +8,-37,-25, +6,-37,-25, +5,-38,-25, +3,-38,-25, +2,-39,-25, +0,-39,-25, +-1,-40,-25, +-2,-41,-25, +-4,-41,-25, +-5,-42,-25, +-7,-42,-25, +-8,-43,-25, +-9,-44,-25, +-11,-44,-25, +-12,-45,-25, +-14,-45,-25, +-15,-46,-25, +-17,-46,-25, +-18,-47,-25, +-19,-48,-25, +-21,-48,-25, +-22,-49,-25, +-24,-49,-25, +-25,-50,-25, +-27,-50,-25, +-29,-50,-25, +-30,-51,-25, +-31,-50,-25, +-33,-50,-25, +-35,-50,-25, +-37,-50,-25, +-39,-50,-25, +-41,-50,-25, +-43,-50,-25, +-44,-51,-25, +-46,-51,-25, +-48,-51,-25, +-50,-51,-25, +-52,-51,-25, +-54,-51,-25, +-55,-52,-25, +-57,-52,-25, +-59,-52,-25, +-61,-52,-25, +-63,-52,-25, +-65,-52,-25, +-67,-52,-25, +-69,-52,-25, +-71,-52,-25, +-70,-50,-25, +-69,-49,-25, +-67,-49,-25, +-66,-48,-25, +-65,-47,-25, +-64,-46,-25, +-63,-45,-25, +-62,-44,-25, +-61,-43,-25, +-60,-42,-25, +-59,-41,-25, +-58,-40,-25, +-57,-39,-25, +-56,-38,-25, +-55,-37,-25, +-54,-36,-25, +-53,-35,-25, +-52,-33,-25, +-51,-32,-25, +-50,-31,-25, +-49,-30,-25, +-50,-29,-25, +-51,-28,-25, +-53,-28,-25, +-55,-28,-25, +-56,-27,-25, +-58,-27,-25, +-59,-26,-25, +-61,-26,-25, +-62,-25,-25, +-64,-25,-25, +-65,-24,-25, +-66,-23,-25, +-67,-22,-25, +-68,-21,-25, +-67,-20,-25, +-66,-19,-25, +-66,-17,-25, +-65,-16,-25, +-64,-15,-25, +-63,-14,-25, +-63,-12,-25, +-62,-11,-25, +-62,-9,-25, +-61,-8,-25, +-61,-6,-25, +-61,-4,-25, +-61,-2,-25, +-62,-1,-25, +-64,-1,-25, +-66,-1,-25, +-67,0,-25, +-69,0,-25, +-70,1,-25, +-72,1,-25, +-73,2,-25, +-74,3,-25, +-75,4,-25, +-76,5,-25, +-78,5,-25, +-78,3,-25, +-78,1,-25, +-78,-1,-25, +-79,-2,-25, +-79,-4,-25, +-80,-6,-25, +-80,-8,-25, +-81,-9,-25, +-82,-10,-25, +-82,-12,-25, +-83,-13,-25, +-84,-14,-25, +-84,-16,-25, +-83,-17,-25, +-82,-18,-25, +-80,-18,-25, +-78,-18,-25, +-76,-18,-25, +-75,-19,-25, +-73,-19,-25, +-72,-20,-25, +-70,-20,-25, +-69,-21,-25, +-68,-22,-25, +-66,-22,-25, +-65,-23,-25, +-64,-24,-25, +-62,-24,-25, +-61,-25,-25, +-59,-25,-25, +-58,-26,-25, +-56,-26,-25, +-55,-27,-25, +-54,-28,-25, +-53,-29,-25, +-53,-31,-25, +-54,-32,-25, +-54,-34,-25, +-55,-35,-25, +-56,-36,-25, +-56,-38,-25, +-57,-39,-25, +-58,-40,-25, +-59,-41,-25, +-60,-42,-25, +-61,-43,-25, +-62,-44,-25, +-63,-45,-25, +-64,-46,-25, +-65,-47,-25, +-66,-49,-25, +-67,-50,-25, +-67,-52,-25, +-68,-53,-25, +-66,-53,-25, +-65,-52,-25, +-63,-52,-25, +-62,-51,-25, +-60,-51,-25, +-58,-51,-25, +-56,-51,-25, +-54,-51,-25, +-52,-51,-25, +-50,-51,-25, +-48,-51,-25, +-46,-51,-25, +-45,-52,-25, +-43,-52,-25, +-42,-51,-25, +-40,-51,-25, +-38,-51,-25, +-36,-51,-25, +-34,-51,-25, +-33,-52,-25, +-33,-54,-25, +-34,-55,-25, +-34,-57,-25, +-34,-59,-25, +-34,-61,-25, +-35,-62,-25, +-36,-63,-25, +-38,-63,-25, +-39,-64,-25, +-41,-64,-25, +-42,-65,-25, +-44,-65,-25, +-45,-66,-25, +-47,-67,-25, +-48,-68,-25, +-49,-69,-25, +-50,-70,-25, +-51,-71,-25, +-52,-72,-25, +-54,-72,-25, +-55,-73,-25, +-56,-74,-25, +-57,-75,-25, +-58,-74,-25, +-58,-72,-25, +-57,-71,-25, +-57,-69,-25, +-56,-68,-25, +-56,-66,-25, +-55,-65,-25, +-55,-63,-25, +-54,-62,-25, +-54,-60,-25, +-54,-58,-25, +-53,-57,-25, +-53,-55,-25, +-53,-53,-25, +-53,-51,-25, +-52,-50,-25, +-52,-48,-25, +-52,-46,-25, +-52,-44,-25, +-51,-43,-25, +-51,-41,-25, +-51,-39,-25, +-50,-38,-25, +-50,-36,-25, +-50,-34,-25, +-49,-33,-25, +-49,-31,-25, +-49,-29,-25, +-48,-28,-25, +-48,-26,-25, +-47,-25,-25, +-46,-24,-25, +-45,-23,-25, +-43,-23,-25, +-42,-22,-25, +-40,-22,-25, +-39,-21,-25, +-38,-20,-25, +-37,-19,-25, +-35,-18,-25, +-34,-17,-25, +-33,-16,-25, +-32,-15,-25, +-31,-14,-25, +-30,-13,-25, +-29,-12,-25, +-28,-11,-25, +-27,-10,-25, +-25,-10,-25, +-24,-9,-25, +-23,-8,-25, +-22,-7,-25, +-21,-6,-25, +-19,-6,-25, +-19,-4,-25, +-18,-3,-25, +-17,-2,-25, +-17,0,-25, +-17,2,-25, +-16,3,-25, +-16,5,-25, +-16,7,-25, +-15,8,-25, +-14,9,-25, +-14,11,-25, +-13,12,-25, +-12,13,-25, +-12,15,-25, +-11,16,-25, +-10,17,-25, +-10,19,-25, +-9,20,-25, +-8,21,-25, +-8,23,-25, +-7,24,-25, +-7,26,-25, +-6,27,-25, +-5,28,-25, +-5,30,-25, +-4,31,-25, +-4,33,-25, +-4,35,-25, +-3,36,-25, +-3,38,-25, +-3,40,-25, +-2,41,-25, +-2,43,-25, +-2,45,-25, +-1,46,-25, +-1,48,-25, +0,49,-25, +0,51,-25, +0,53,-25, +-1,54,-25, +-2,55,-25, +-3,56,-25, +-4,57,-25, +-6,57,-25, +-7,58,-25, +-8,59,-25, +-9,60,-25, +-10,61,-25, +-10,63,-25, +-11,64,-25, +-12,65,-25, +-13,66,-25, +-13,68,-25, +-14,69,-25, +-15,70,-25, +-14,71,-25, +-12,71,-25, +-10,70,-25, +-8,70,-25, +-6,70,-25, +-5,69,-25, +-3,69,-25, +-1,69,-25, +1,69,-25, +3,69,-25, +5,69,-25, +6,70,-25, +6,68,-25, +5,67,-25, +5,65,-25, +4,64,-25, +3,62,-25, +3,60,-25, +2,59,-25, +1,58,-25, +1,56,-25, +1,54,-25, +0,53,-25, +0,51,-25, +-1,50,-25, +-1,48,-25, +-1,46,-25, +0,45,-25, +1,44,-25, +3,44,-25, +4,43,-25, +6,43,-25, +7,42,-25, +9,42,-25, +10,41,-25, +11,40,-25, +12,39,-25, +14,39,-25, +15,38,-25, +16,37,-25, +17,36,-25, +18,35,-25, +19,34,-25, +21,34,-25, +22,33,-25, +23,32,-25, +24,31,-25, +26,31,-25, +27,30,-25, +28,29,-25, +30,29,-25, +31,28,-25, +32,27,-25, +33,26,-25, +35,26,-25, +36,25,-25, +37,24,-25, +38,23,-25, +40,23,-25, +40,21,-25, +41,20,-25, +41,18,-25, +41,16,-25, +41,14,-25, +42,13,-25, +42,11,-25, +42,9,-25, +43,8,-25, +43,6,-25, +44,5,-25, +45,4,-25, +45,2,-25, +46,1,-25, +46,-1,-25, +47,-2,-25, +47,-4,-25, +48,-5,-25, +48,-7,-25, +49,-8,-25, +49,-10,-25, +50,-11,-25, +50,-13,-25, +51,-14,-25, +51,-16,-25, +52,-17,-25, +52,-19,-25, +53,-20,-25, +52,-21,-25, +52,-23,-25, +51,-24,-25, +51,-26,-25, +50,-27,-25, +49,-28,-25, +49,-30,-25, +48,-31,-25, +48,-33,-25, +47,-34,-25, +47,-36,-25, +46,-37,-25, +46,-39,-25, +46,-41,-25, +45,-42,-25, +45,-44,-25, +46,-45,-25, +46,-47,-25, +47,-48,-25, +48,-50,-25, +49,-51,-25, +49,-53,-25, +50,-54,-25, +51,-55,-25, +51,-57,-25, +52,-58,-25, +52,-60,-25, +52,-62,-25, +53,-63,-25, +53,-65,-25, +54,-66,-25, +54,-68,-25, +54,-70,-25, +55,-71,-25, +55,-73,-25, +56,-74,-25, +57,-75,-25, +57,-77,-25, +58,-78,-25, +58,-80,-25, +57,-81,-25, +56,-82,-25, +54,-82,-25, +53,-83,-25, +51,-83,-25, +50,-84,-25, +49,-85,-25, +47,-85,-25, +46,-86,-25, +45,-87,-25, +44,-88,-25, +43,-89,-25, +41,-90,-25, +40,-91,-25, +39,-92,-25, +38,-93,-25, +36,-93,-25, +35,-94,-25, +34,-95,-25, +33,-96,-25, +32,-97,-25, +30,-97,-25, +29,-98,-25, +28,-99,-25, +27,-100,-25, +25,-100,-25, +24,-101,-25, +23,-102,-25, +22,-103,-25, +21,-104,-25, +19,-104,-25, +18,-105,-25, +18,-107,-25, +19,-108,-25, +21,-108,-25, +22,-109,-25, +23,-110,-25, +25,-110,-25, +26,-111,-25, +27,-112,-25, +28,-113,-25, +29,-114,-25, +30,-115,-25, +31,-116,-25, +32,-117,-25, +33,-118,-25, +34,-119,-25, +35,-120,-25, +36,-122,-25, +38,-123,-25, +40,-122,-25, +43,-122,-25, +45,-121,-25, +47,-120,-25, +49,-119,-25, +51,-120,-25, +52,-122,-25, +52,-125,-25, +53,-127,-25, +53,-130,-25, +54,-132,-25, +55,-134,-25, +57,-135,-25, +58,-137,-25, +59,-139,-25, +61,-141,-25, +62,-143,-25, +63,-145,-25, +64,-147,-25, +63,-149,-25, +62,-151,-25, +61,-153,-25, +60,-155,-25, +59,-157,-25, +58,-159,-25, +57,-161,-25, +57,-164,-25, +56,-166,-25, +56,-169,-25, +55,-171,-25, +55,-174,-25, +54,-176,-25, +53,-178,-25, +53,-181,-25, +52,-183,-25, +51,-185,-25, +49,-186,-25, +48,-188,-25, +46,-189,-25, +44,-190,-25, +43,-192,-25, +42,-194,-25, +40,-195,-25, +39,-197,-25, +38,-199,-25, +36,-200,-25, +35,-202,-25, +34,-204,-25, +32,-205,-25, +31,-207,-25, +30,-205,-25, +30,-202,-25, +31,-200,-25, +31,-197,-25, +31,-194,-25, +32,-192,-25, +32,-189,-25, +32,-186,-25, +32,-183,-25, +32,-180,-25, +32,-177,-25, +32,-174,-25, +32,-171,-25, +32,-168,-25, +33,-166,-25, +33,-163,-25, +33,-160,-25, +33,-157,-25, +33,-154,-25, +33,-151,-25, +33,-148,-25, +34,-146,-25, +34,-143,-25, +34,-140,-25, +34,-137,-25, +34,-134,-25, +34,-131,-25, +34,-128,-25, +35,-126,-25, +35,-123,-25, +35,-120,-25, +34,-119,-25, +33,-118,-25, +32,-117,-25, +31,-116,-25, +30,-115,-25, +29,-114,-25, +28,-113,-25, +27,-112,-25, +26,-111,-25, +25,-110,-25, +24,-109,-25, +23,-108,-25, +23,-106,-25, +22,-105,-25, +23,-104,-25, +24,-103,-25, +25,-102,-25, +26,-101,-25, +27,-100,-25, +29,-100,-25, +30,-99,-25, +31,-98,-25, +32,-97,-25, +33,-96,-25, +33,-94,-25, +34,-93,-25, +35,-92,-25, +35,-94,-25, +36,-95,-25, +36,-97,-25, +37,-98,-25, +38,-99,-25, +38,-101,-25, +39,-102,-25, +40,-103,-25, +40,-105,-25, +41,-106,-25, +42,-107,-25, +43,-108,-25, +45,-108,-25, +46,-107,-25, +47,-106,-25, +49,-106,-25, +50,-105,-25, +51,-104,-25, +52,-103,-25, +53,-102,-25, +55,-102,-25, +57,-102,-25, +58,-103,-25, +59,-104,-25, +60,-105,-25, +61,-106,-25, +62,-107,-25, +64,-107,-25, +65,-109,-25, +68,-109,-25, +70,-110,-25, +72,-111,-25, +75,-111,-25, +77,-112,-25, +79,-113,-25, +81,-114,-25, +84,-114,-25, +86,-115,-25, +89,-115,-25, +91,-114,-25, +93,-113,-25, +95,-112,-25, +97,-111,-25, +99,-110,-25, +102,-110,-25, +104,-109,-25, +105,-107,-25, +104,-105,-25, +104,-102,-25, +104,-99,-25, +104,-96,-25, +105,-94,-25, +106,-92,-25, +106,-89,-25, +107,-87,-25, +107,-84,-25, +105,-83,-25, +104,-81,-25, +102,-80,-25, +100,-79,-25, +98,-78,-25, +99,-77,-25, +101,-76,-25, +103,-75,-25, +106,-75,-25, +108,-74,-25, +111,-74,-25, +113,-73,-25, +114,-75,-25, +115,-77,-25, +116,-79,-25, +117,-82,-25, +116,-84,-25, +115,-86,-25, +113,-87,-25, +112,-89,-25, +110,-90,-25, +109,-92,-25, +108,-94,-25, +107,-92,-25, +106,-90,-25, +105,-88,-25, +104,-86,-25, +103,-84,-25, +102,-82,-25, +101,-80,-25, +99,-79,-25, +99,-77,-25, +98,-76,-25, +97,-75,-25, +96,-74,-25, +95,-73,-25, +93,-73,-25, +92,-74,-25, +90,-75,-25, +88,-75,-25, +87,-76,-25, +85,-76,-25, +84,-77,-25, +82,-77,-25, +80,-77,-25, +78,-77,-25, +76,-77,-25, +79,-76,-25, +80,-75,-25, +82,-75,-25, +84,-75,-25, +86,-75,-25, +87,-74,-25, +89,-74,-25, +91,-74,-25, +92,-73,-25, +94,-73,-25, +95,-72,-25, +97,-72,-25, +99,-72,-25, +100,-71,-25, +102,-71,-25, +103,-70,-25, +106,-70,-25, +108,-69,-25, +111,-69,-25, +113,-68,-25, +115,-67,-25, +118,-67,-25, +120,-66,-25, +121,-64,-25, +119,-63,-25, +118,-61,-25, +117,-59,-25, +115,-58,-25, +114,-56,-25, +113,-54,-25, +113,-52,-25, +112,-51,-25, +111,-50,-25, +111,-48,-25, +110,-47,-25, +110,-45,-25, +109,-44,-25, +108,-43,-25, +108,-41,-25, +107,-40,-25, +106,-39,-25, +105,-38,-25, +105,-36,-25, +104,-35,-25, +103,-34,-25, +103,-32,-25, +101,-32,-25, +99,-32,-25, +98,-33,-25, +97,-34,-25, +95,-34,-25, +94,-35,-25, +93,-36,-25, +91,-36,-25, +89,-36,-25, +87,-36,-25, +85,-36,-25, +83,-36,-25, +81,-36,-25, +80,-35,-25, +79,-34,-25, +78,-33,-25, +76,-32,-25, +75,-31,-25, +73,-31,-25, +72,-30,-25, +71,-29,-25, +69,-29,-25, +69,-27,-25, +69,-25,-25, +70,-24,-25, +70,-22,-25, +70,-20,-25, +70,-18,-25, +70,-16,-25, +70,-14,-25, +70,-12,-25, +69,-11,-25, +69,-9,-25, +71,-9,-25, +73,-9,-25, +75,-9,-25, +77,-9,-25, +79,-9,-25, +81,-9,-25, +82,-8,-25, +84,-8,-25, +86,-8,-25, +87,-7,-25, +88,-8,-25, +89,-9,-25, +90,-10,-25, +91,-11,-25, +92,-12,-25, +93,-13,-25, +94,-14,-25, +95,-15,-25, +96,-16,-25, +97,-17,-25, +98,-18,-25, +100,-18,-25, +101,-19,-25, +103,-19,-25, +104,-20,-25, +104,-22,-25, +103,-23,-25, +103,-25,-25, +102,-26,-25, +101,-27,-25, +100,-28,-25, +100,-30,-25, +99,-31,-25, +99,-33,-25, +98,-34,-25, +98,-36,-25, +100,-36,-25, +100,-34,-25, +101,-33,-25, +102,-32,-25, +103,-31,-25, +104,-30,-25, +104,-28,-25, +105,-27,-25, +106,-26,-25, +107,-25,-25, +108,-24,-25, +108,-22,-25, +109,-21,-25, +110,-20,-25, +111,-19,-25, +112,-18,-25, +113,-17,-25, +114,-16,-25, +115,-15,-25, +116,-14,-25, +117,-13,-25, +117,-11,-25, +117,-9,-25, +116,-8,-25, +115,-7,-25, +114,-6,-25, +113,-5,-25, +112,-4,-25, +111,-3,-25, +110,-2,-25, +110,0,-25, +109,1,-25, +108,2,-25, +108,4,-25, +108,6,-25, +109,7,-25, +109,9,-25, +109,11,-25, +109,13,-25, +109,15,-25, +108,16,-25, +108,18,-25, +108,20,-25, +107,21,-25, +107,23,-25, +107,25,-25, +107,27,-25, +107,29,-25, +106,30,-25, +106,32,-25, +106,34,-25, +104,33,-25, +103,32,-25, +102,31,-25, +100,31,-25, +99,30,-25, +98,29,-25, +96,29,-25, +95,28,-25, +94,27,-25, +92,27,-25, +91,26,-25, +89,26,-25, +87,26,-25, +86,25,-25, +84,25,-25, +83,24,-25, +81,24,-25, +80,23,-25, +78,23,-25, +77,22,-25, +75,22,-25, +74,21,-25, +73,20,-25, +71,20,-25, +71,18,-25, +72,16,-25, +73,15,-25, +73,13,-25, +74,12,-25, +74,10,-25, +75,9,-25, +75,7,-25, +76,6,-25, +76,4,-25, +76,2,-25, +76,0,-25, +77,-1,-25, +77,-3,-25, +77,-5,-25, +77,-7,-25, +78,-8,-25, +78,-10,-25, +79,-11,-25, +79,-13,-25, +80,-14,-25, +80,-16,-25, +80,-18,-25, +81,-19,-25, +81,-21,-25, +81,-23,-25, +82,-24,-25, diff --git a/src/modules/navigator/navigator_tests/long_path2.path b/src/modules/navigator/navigator_tests/long_path2.path new file mode 100644 index 0000000000..abbde41a3c --- /dev/null +++ b/src/modules/navigator/navigator_tests/long_path2.path @@ -0,0 +1,2442 @@ +0,0,0, +0,0,-2, +1,0,-3, +2,0,-4, +3,0,-5, +5,0,-5, +7,0,-5, +8,1,-5, +10,1,-5, +12,1,-5, +14,1,-5, +14,3,-5, +13,4,-5, +12,5,-5, +11,6,-5, +10,7,-5, +9,8,-5, +9,10,-5, +8,11,-5, +8,13,-5, +7,14,-5, +7,16,-5, +6,17,-5, +5,16,-5, +4,15,-5, +4,13,-5, +3,12,-5, +2,11,-5, +1,10,-5, +0,9,-5, +-1,8,-5, +-2,7,-5, +-3,6,-5, +-4,5,-5, +-5,4,-5, +-6,3,-5, +-8,3,-5, +-9,2,-5, +-10,1,-5, +-8,1,-5, +-7,2,-5, +-6,3,-5, +-5,4,-5, +-4,5,-5, +-2,5,-5, +-1,6,-5, +0,7,-5, +1,8,-5, +3,8,-5, +4,9,-5, +5,10,-5, +7,10,-5, +8,11,-5, +9,12,-5, +10,13,-5, +12,13,-5, +13,14,-5, +14,15,-5, +15,16,-5, +17,16,-5, +18,17,-5, +19,18,-5, +20,19,-5, +19,20,-5, +19,22,-5, +18,23,-5, +17,24,-5, +16,25,-5, +15,26,-5, +15,28,-5, +14,29,-5, +14,31,-5, +13,32,-5, +13,34,-5, +12,35,-5, +11,34,-5, +10,33,-5, +9,32,-5, +7,32,-5, +6,31,-5, +4,30,-5, +2,30,-5, +0,30,-5, +4,30,-5, +6,30,-5, +8,30,-5, +10,30,-5, +11,29,-5, +13,29,-5, +14,28,-5, +16,28,-5, +17,27,-5, +19,27,-5, +20,26,-5, +22,26,-5, +24,26,-5, +25,25,-5, +27,25,-5, +28,26,-5, +27,27,-5, +26,28,-5, +25,29,-5, +24,30,-5, +23,31,-5, +22,33,-5, +21,34,-5, +20,35,-5, +20,37,-5, +19,38,-5, +18,39,-5, +18,41,-5, +17,42,-5, +16,44,-5, +15,45,-5, +15,47,-5, +14,48,-5, +13,49,-5, +12,50,-5, +11,51,-5, +11,53,-5, +10,54,-5, +9,55,-5, +8,56,-5, +10,56,-5, +11,54,-5, +12,53,-5, +13,52,-5, +14,51,-5, +15,50,-5, +16,49,-5, +17,48,-5, +18,47,-5, +19,46,-5, +20,45,-5, +21,44,-5, +22,43,-5, +23,42,-5, +24,41,-5, +25,40,-5, +26,39,-5, +27,38,-5, +28,37,-5, +29,36,-5, +30,35,-5, +31,34,-5, +32,33,-5, +33,32,-5, +34,31,-5, +35,30,-5, +36,29,-5, +36,27,-5, +35,26,-5, +34,25,-5, +33,24,-5, +32,23,-5, +32,21,-5, +31,20,-5, +30,19,-5, +30,17,-5, +29,16,-5, +29,14,-5, +28,13,-5, +28,11,-5, +28,9,-5, +27,8,-5, +27,6,-5, +26,5,-5, +26,3,-5, +25,2,-5, +24,1,-5, +24,-1,-5, +23,-2,-5, +23,-4,-5, +22,-5,-5, +22,-7,-5, +21,-8,-5, +21,-6,-5, +21,-4,-5, +21,-2,-5, +21,0,-5, +21,2,-5, +21,4,-5, +20,5,-5, +20,7,-5, +20,9,-5, +20,11,-5, +20,13,-5, +20,15,-5, +20,17,-5, +19,18,-5, +19,20,-5, +19,22,-5, +19,24,-5, +18,25,-5, +18,27,-5, +18,29,-5, +18,31,-5, +17,28,-5, +16,27,-5, +16,25,-5, +15,24,-5, +15,22,-5, +14,21,-5, +13,20,-5, +13,18,-5, +12,17,-5, +11,16,-5, +11,14,-5, +10,13,-5, +9,12,-5, +8,11,-5, +8,9,-5, +7,8,-5, +7,6,-5, +6,5,-5, +5,4,-5, +5,2,-5, +4,1,-5, +3,0,-5, +3,-2,-5, +2,-3,-5, +1,-4,-5, +1,-6,-5, +0,-7,-5, +-1,-8,-5, +-1,-10,-5, +-2,-11,-5, +-3,-12,-5, +-3,-10,-5, +-2,-9,-5, +-2,-7,-5, +-2,-5,-5, +-2,-3,-5, +-1,-2,-5, +-1,0,-5, +-1,2,-5, +-1,4,-5, +-1,6,-5, +-1,8,-5, +-1,10,-5, +-1,12,-5, +-1,14,-5, +-1,16,-5, +0,17,-5, +0,19,-5, +0,21,-5, +0,23,-5, +0,25,-5, +0,27,-5, +0,29,-5, +0,31,-5, +0,33,-5, +1,34,-5, +1,36,-5, +1,38,-5, +0,39,-5, +0,37,-5, +-1,36,-5, +-2,35,-5, +-2,33,-5, +-3,32,-5, +-4,31,-5, +-5,30,-5, +-5,28,-5, +-6,27,-5, +-7,26,-5, +-8,25,-5, +-8,23,-5, +-9,22,-5, +-10,21,-5, +-11,20,-5, +-12,19,-5, +-12,17,-5, +-11,16,-5, +-10,15,-5, +-8,15,-5, +-7,14,-5, +-6,13,-5, +-5,12,-5, +-4,11,-5, +-3,10,-5, +-2,9,-5, +-1,8,-5, +0,7,-5, +1,6,-5, +2,5,-5, +3,4,-5, +4,3,-5, +5,2,-5, +5,0,-5, +6,-1,-5, +7,-2,-5, +8,-3,-5, +9,-4,-5, +10,-5,-5, +11,-6,-5, +12,-7,-5, +13,-8,-5, +14,-9,-5, +15,-10,-5, +16,-11,-5, +17,-12,-5, +18,-13,-5, +18,-15,-5, +19,-16,-5, +20,-17,-5, +21,-18,-5, +22,-19,-5, +23,-20,-5, +23,-22,-5, +24,-23,-5, +23,-24,-5, +21,-24,-5, +19,-24,-5, +17,-24,-5, +16,-25,-5, +14,-25,-5, +12,-25,-5, +11,-26,-5, +9,-26,-5, +7,-26,-5, +6,-27,-5, +4,-28,-5, +2,-28,-5, +1,-29,-5, +-1,-29,-5, +-2,-30,-5, +-3,-29,-5, +-2,-28,-5, +-1,-27,-5, +0,-26,-5, +1,-25,-5, +2,-24,-5, +3,-23,-5, +5,-23,-5, +6,-22,-5, +7,-21,-5, +8,-20,-5, +9,-19,-5, +10,-18,-5, +11,-17,-5, +12,-16,-5, +13,-15,-5, +14,-14,-5, +15,-13,-5, +16,-12,-5, +17,-11,-5, +17,-9,-5, +18,-8,-5, +19,-7,-5, +20,-6,-5, +21,-5,-5, +22,-4,-5, +23,-3,-5, +24,-2,-5, +25,-1,-5, +26,0,-5, +27,1,-5, +28,2,-5, +29,3,-5, +30,4,-5, +31,5,-5, +32,6,-5, +33,7,-5, +34,8,-5, +35,9,-5, +36,10,-5, +37,11,-5, +38,12,-5, +39,13,-5, +40,14,-5, +41,15,-5, +42,16,-5, +43,17,-5, +44,18,-5, +44,20,-5, +43,21,-5, +43,23,-5, +42,24,-5, +41,25,-5, +41,27,-5, +40,28,-5, +40,30,-5, +40,32,-5, +39,33,-5, +39,35,-5, +39,37,-5, +39,39,-5, +39,41,-5, +39,43,-5, +39,45,-5, +38,46,-5, +38,48,-5, +38,50,-5, +37,51,-5, +35,51,-5, +33,51,-5, +31,51,-5, +29,51,-5, +27,51,-5, +25,51,-5, +23,51,-5, +21,51,-5, +21,49,-5, +20,48,-5, +20,46,-5, +19,45,-5, +18,44,-5, +17,43,-5, +16,42,-5, +15,40,-5, +14,39,-5, +13,38,-5, +12,37,-5, +11,36,-5, +10,35,-5, +9,34,-5, +8,33,-5, +7,32,-5, +6,31,-5, +5,30,-5, +4,29,-5, +3,28,-5, +2,27,-5, +1,26,-5, +0,25,-5, +-1,24,-5, +-2,23,-5, +-3,22,-5, +-4,21,-5, +-5,20,-5, +-6,19,-5, +-7,18,-5, +-8,17,-5, +-9,16,-5, +-10,15,-5, +-11,14,-5, +-12,13,-5, +-13,11,-5, +-14,10,-5, +-15,9,-5, +-14,8,-5, +-12,8,-5, +-10,8,-5, +-8,8,-5, +-7,7,-5, +-5,7,-5, +-3,7,-5, +-2,6,-5, +0,6,-5, +1,5,-5, +3,5,-5, +4,4,-5, +6,4,-5, +7,3,-5, +9,3,-5, +11,3,-5, +12,2,-5, +14,2,-5, +16,2,-5, +17,1,-5, +19,1,-5, +21,1,-5, +22,0,-5, +24,0,-5, +25,-1,-5, +27,-1,-5, +29,-1,-5, +30,-2,-5, +32,-2,-5, +34,-2,-5, +35,-3,-5, +35,-5,-5, +34,-6,-5, +33,-7,-5, +32,-8,-5, +31,-9,-5, +31,-11,-5, +30,-12,-5, +29,-13,-5, +28,-14,-5, +28,-16,-5, +27,-17,-5, +27,-19,-5, +26,-20,-5, +26,-22,-5, +25,-23,-5, +25,-25,-5, +24,-26,-5, +23,-27,-5, +23,-29,-5, +22,-30,-5, +21,-32,-5, +21,-34,-5, +20,-35,-5, +19,-36,-5, +19,-38,-5, +18,-39,-5, +21,-38,-5, +22,-37,-5, +23,-36,-5, +24,-35,-5, +25,-34,-5, +26,-33,-5, +28,-33,-5, +29,-32,-5, +30,-31,-5, +32,-31,-5, +33,-30,-5, +35,-30,-5, +36,-29,-5, +37,-28,-5, +39,-28,-5, +40,-27,-5, +41,-26,-5, +43,-26,-5, +44,-25,-5, +45,-24,-5, +46,-23,-5, +48,-23,-5, +49,-22,-5, +50,-21,-5, +52,-20,-5, +53,-19,-5, +55,-19,-5, +56,-18,-5, +57,-17,-5, +58,-16,-5, +58,-14,-5, +57,-13,-5, +57,-11,-5, +56,-10,-5, +55,-9,-5, +55,-7,-5, +55,-5,-5, +54,-4,-5, +54,-2,-5, +54,0,-5, +54,2,-5, +54,4,-5, +54,6,-5, +54,8,-5, +54,10,-5, +54,12,-5, +54,14,-5, +53,15,-5, +53,17,-5, +53,19,-5, +53,21,-5, +53,23,-5, +52,24,-5, +52,26,-5, +52,28,-5, +53,29,-5, +53,31,-5, +54,32,-5, +55,33,-5, +55,35,-5, +56,36,-5, +57,37,-5, +57,39,-5, +58,40,-5, +58,42,-5, +59,43,-5, +59,45,-5, +60,46,-5, +60,48,-5, +61,50,-5, +61,52,-5, +62,53,-5, +62,55,-5, +63,56,-5, +63,58,-5, +64,59,-5, +64,61,-5, +65,62,-5, +65,64,-5, +66,65,-5, +66,67,-5, +67,68,-5, +66,69,-5, +65,68,-5, +64,67,-5, +62,67,-5, +61,66,-5, +60,65,-5, +58,65,-5, +57,64,-5, +55,64,-5, +54,63,-5, +52,63,-5, +50,63,-5, +49,62,-5, +47,62,-5, +45,62,-5, +44,61,-5, +42,61,-5, +41,60,-5, +39,60,-5, +38,59,-5, +36,59,-5, +35,58,-5, +33,58,-5, +31,58,-5, +30,57,-5, +28,57,-5, +27,56,-5, +25,56,-5, +24,55,-5, +22,55,-5, +21,54,-5, +19,54,-5, +17,54,-5, +16,53,-5, +14,53,-5, +13,52,-5, +11,52,-5, +10,51,-5, +8,51,-5, +7,50,-5, +5,50,-5, +4,49,-5, +2,49,-5, +0,49,-5, +-1,48,-5, +-2,47,-5, +-4,47,-5, +-5,46,-5, +-6,45,-5, +-7,44,-5, +-8,43,-5, +-9,42,-5, +-10,41,-5, +-11,40,-5, +-12,39,-5, +-13,38,-5, +-15,38,-5, +-16,37,-5, +-17,36,-5, +-18,35,-5, +-19,34,-5, +-21,34,-5, +-22,33,-5, +-23,32,-5, +-24,31,-5, +-25,30,-5, +-27,30,-5, +-28,29,-5, +-29,28,-5, +-30,27,-5, +-31,26,-5, +-32,25,-5, +-34,25,-5, +-35,24,-5, +-36,23,-5, +-37,22,-5, +-38,21,-5, +-39,20,-5, +-41,20,-5, +-42,19,-5, +-43,18,-5, +-44,17,-5, +-45,16,-5, +-46,15,-5, +-48,15,-5, +-49,14,-5, +-50,13,-5, +-51,12,-5, +-52,11,-5, +-53,10,-5, +-55,10,-5, +-56,9,-5, +-57,8,-5, +-58,7,-5, +-59,6,-5, +-61,5,-5, +-62,4,-5, +-63,3,-5, +-65,3,-5, +-66,2,-5, +-66,0,-5, +-65,-1,-5, +-64,-2,-5, +-62,-2,-5, +-61,-3,-5, +-60,-4,-5, +-59,-5,-5, +-58,-6,-5, +-57,-7,-5, +-56,-8,-5, +-55,-9,-5, +-55,-11,-5, +-54,-12,-5, +-53,-13,-5, +-52,-14,-5, +-51,-15,-5, +-50,-16,-5, +-50,-18,-5, +-49,-19,-5, +-48,-20,-5, +-47,-21,-5, +-46,-22,-5, +-45,-23,-5, +-44,-24,-5, +-43,-25,-5, +-42,-26,-5, +-41,-27,-5, +-40,-28,-5, +-39,-29,-5, +-38,-30,-5, +-37,-31,-5, +-36,-32,-5, +-35,-33,-5, +-34,-34,-5, +-34,-36,-5, +-33,-37,-5, +-32,-38,-5, +-31,-39,-5, +-30,-40,-5, +-29,-41,-5, +-28,-42,-5, +-27,-43,-5, +-26,-44,-5, +-25,-45,-5, +-24,-46,-5, +-23,-47,-5, +-24,-44,-5, +-25,-43,-5, +-26,-42,-5, +-26,-40,-5, +-27,-39,-5, +-27,-37,-5, +-28,-36,-5, +-28,-34,-5, +-29,-33,-5, +-29,-31,-5, +-30,-30,-5, +-30,-28,-5, +-30,-26,-5, +-31,-25,-5, +-31,-23,-5, +-32,-22,-5, +-33,-21,-5, +-33,-19,-5, +-34,-18,-5, +-34,-16,-5, +-35,-15,-5, +-35,-13,-5, +-36,-12,-5, +-36,-10,-5, +-37,-9,-5, +-37,-7,-5, +-38,-6,-5, +-38,-4,-5, +-39,-3,-5, +-39,-1,-5, +-40,0,-5, +-40,2,-5, +-41,3,-5, +-41,5,-5, +-42,6,-5, +-42,8,-5, +-43,9,-5, +-43,11,-5, +-44,12,-5, +-44,14,-5, +-45,15,-5, +-45,17,-5, +-46,18,-5, +-46,20,-5, +-47,21,-5, +-47,23,-5, +-46,25,-5, +-45,27,-5, +-44,28,-5, +-43,30,-5, +-42,31,-5, +-42,33,-5, +-41,34,-5, +-41,36,-5, +-40,37,-5, +-40,39,-5, +-39,40,-5, +-39,42,-5, +-39,44,-5, +-38,45,-5, +-38,47,-5, +-37,48,-5, +-37,50,-5, +-36,51,-5, +-36,53,-5, +-35,54,-5, +-34,55,-5, +-34,57,-5, +-33,58,-5, +-33,60,-5, +-32,61,-5, +-32,63,-5, +-31,64,-5, +-31,66,-5, +-30,67,-5, +-30,69,-5, +-29,70,-5, +-29,72,-5, +-28,73,-5, +-27,74,-5, +-25,74,-5, +-24,75,-5, +-22,75,-5, +-21,76,-5, +-19,76,-5, +-18,77,-5, +-16,77,-5, +-15,78,-5, +-14,79,-5, +-12,79,-5, +-11,80,-5, +-10,81,-5, +-9,82,-5, +-8,83,-5, +-6,84,-5, +-5,85,-5, +-3,85,-5, +-2,86,-5, +-1,87,-5, +1,87,-5, +2,88,-5, +3,89,-5, +5,89,-5, +6,90,-5, +7,91,-5, +8,92,-5, +10,92,-5, +11,93,-5, +12,94,-5, +14,94,-5, +15,95,-5, +16,96,-5, +17,97,-5, +19,97,-5, +20,98,-5, +21,99,-5, +23,99,-5, +24,100,-5, +25,101,-5, +26,102,-5, +28,102,-5, +29,103,-5, +30,104,-5, +32,104,-5, +33,105,-5, +34,106,-5, +35,107,-5, +37,107,-5, +38,108,-5, +39,109,-5, +41,109,-5, +42,110,-5, +43,111,-5, +44,112,-5, +46,112,-5, +46,110,-5, +47,109,-5, +47,107,-5, +47,105,-5, +47,103,-5, +47,101,-5, +48,100,-5, +48,98,-5, +49,97,-5, +49,95,-5, +50,94,-5, +50,92,-5, +51,91,-5, +51,89,-5, +52,88,-5, +52,86,-5, +53,84,-5, +53,82,-5, +54,81,-5, +54,79,-5, +54,77,-5, +55,76,-5, +55,74,-5, +56,73,-5, +56,71,-5, +56,69,-5, +57,68,-5, +57,66,-5, +58,65,-5, +58,63,-5, +59,62,-5, +59,60,-5, +60,59,-5, +60,57,-5, +60,55,-5, +61,54,-5, +61,52,-5, +62,51,-5, +62,49,-5, +62,47,-5, +63,46,-5, +63,44,-5, +64,43,-5, +64,41,-5, +65,40,-5, +65,38,-5, +65,36,-5, +66,35,-5, +66,33,-5, +67,32,-5, +67,30,-5, +68,29,-5, +68,27,-5, +68,25,-5, +69,24,-5, +69,22,-5, +70,21,-5, +70,19,-5, +70,17,-5, +69,16,-5, +69,14,-5, +68,13,-5, +67,12,-5, +66,11,-5, +65,10,-5, +64,9,-5, +63,8,-5, +62,7,-5, +62,5,-5, +61,4,-5, +60,3,-5, +60,1,-5, +59,0,-5, +59,-2,-5, +58,-3,-5, +57,-4,-5, +57,-6,-5, +56,-7,-5, +55,-8,-5, +54,-9,-5, +54,-11,-5, +53,-12,-5, +52,-13,-5, +51,-14,-5, +51,-16,-5, +50,-17,-5, +49,-18,-5, +49,-20,-5, +48,-21,-5, +47,-22,-5, +47,-24,-5, +46,-25,-5, +45,-26,-5, +44,-27,-5, +44,-29,-5, +43,-30,-5, +42,-31,-5, +42,-33,-5, +41,-34,-5, +40,-35,-5, +39,-36,-5, +39,-38,-5, +38,-39,-5, +37,-40,-5, +37,-42,-5, +36,-43,-5, +35,-44,-5, +34,-45,-5, +34,-47,-5, +33,-48,-5, +32,-49,-5, +32,-51,-5, +31,-52,-5, +30,-53,-5, +29,-55,-5, +28,-56,-5, +28,-58,-5, +27,-59,-5, +26,-60,-5, +26,-62,-5, +25,-63,-5, +24,-64,-5, +23,-65,-5, +23,-67,-5, +22,-68,-5, +21,-69,-5, +21,-71,-5, +20,-72,-5, +19,-73,-5, +19,-75,-5, +18,-76,-5, +17,-77,-5, +16,-78,-5, +16,-80,-5, +15,-81,-5, +14,-82,-5, +14,-84,-5, +13,-85,-5, +12,-86,-5, +11,-87,-5, +11,-89,-5, +10,-90,-5, +9,-91,-5, +9,-93,-5, +8,-94,-5, +7,-95,-5, +6,-96,-5, +6,-98,-5, +5,-99,-5, +4,-100,-5, +3,-99,-5, +3,-97,-5, +3,-95,-5, +2,-94,-5, +2,-92,-5, +2,-90,-5, +1,-89,-5, +1,-87,-5, +0,-86,-5, +0,-84,-5, +-1,-83,-5, +-1,-81,-5, +-2,-80,-5, +-2,-78,-5, +-3,-77,-5, +-3,-75,-5, +-4,-74,-5, +-4,-72,-5, +-5,-71,-5, +-5,-69,-5, +-6,-68,-5, +-6,-66,-5, +-6,-64,-5, +-7,-63,-5, +-7,-61,-5, +-8,-60,-5, +-8,-58,-5, +-9,-57,-5, +-9,-55,-5, +-10,-54,-5, +-10,-52,-5, +-11,-51,-5, +-11,-49,-5, +-12,-48,-5, +-12,-46,-5, +-13,-45,-5, +-13,-43,-5, +-14,-42,-5, +-14,-40,-5, +-15,-39,-5, +-15,-37,-5, +-15,-35,-5, +-16,-34,-5, +-16,-32,-5, +-17,-31,-5, +-17,-29,-5, +-18,-28,-5, +-18,-26,-5, +-19,-25,-5, +-19,-23,-5, +-20,-22,-5, +-20,-20,-5, +-21,-19,-5, +-21,-17,-5, +-22,-16,-5, +-22,-14,-5, +-24,-14,-5, +-25,-15,-5, +-26,-16,-5, +-27,-17,-5, +-28,-18,-5, +-29,-19,-5, +-30,-20,-5, +-31,-21,-5, +-33,-21,-5, +-34,-22,-5, +-35,-23,-5, +-37,-23,-5, +-38,-24,-5, +-40,-24,-5, +-41,-25,-5, +-43,-25,-5, +-44,-26,-5, +-45,-27,-5, +-47,-28,-5, +-48,-29,-5, +-50,-29,-5, +-51,-30,-5, +-52,-31,-5, +-54,-31,-5, +-55,-32,-5, +-56,-33,-5, +-57,-34,-5, +-59,-34,-5, +-60,-35,-5, +-61,-36,-5, +-63,-36,-5, +-64,-37,-5, +-65,-38,-5, +-67,-38,-5, +-68,-39,-5, +-69,-40,-5, +-71,-40,-5, +-72,-41,-5, +-73,-42,-5, +-74,-43,-5, +-73,-44,-5, +-72,-46,-5, +-71,-47,-5, +-71,-49,-5, +-70,-50,-5, +-69,-51,-5, +-69,-53,-5, +-68,-54,-5, +-68,-56,-5, +-68,-58,-5, +-67,-59,-5, +-67,-61,-5, +-67,-63,-5, +-67,-65,-5, +-66,-66,-5, +-66,-68,-5, +-65,-69,-5, +-65,-71,-5, +-64,-72,-5, +-64,-74,-5, +-63,-75,-5, +-63,-77,-5, +-62,-79,-5, +-62,-81,-5, +-61,-82,-5, +-61,-84,-5, +-61,-86,-5, +-60,-87,-5, +-60,-89,-5, +-59,-90,-5, +-59,-92,-5, +-59,-94,-5, +-58,-95,-5, +-58,-97,-5, +-57,-98,-5, +-57,-100,-5, +-56,-101,-5, +-56,-103,-5, +-56,-105,-5, +-55,-106,-5, +-55,-108,-5, +-54,-109,-5, +-54,-111,-5, +-53,-112,-5, +-53,-114,-5, +-53,-116,-5, +-52,-118,-5, +-51,-120,-5, +-51,-123,-5, +-50,-125,-5, +-49,-127,-5, +-49,-130,-5, +-48,-132,-5, +-47,-134,-5, +-47,-137,-5, +-46,-139,-5, +-45,-141,-5, +-45,-144,-5, +-44,-146,-5, +-42,-147,-5, +-40,-146,-5, +-37,-146,-5, +-35,-145,-5, +-32,-145,-5, +-29,-145,-5, +-26,-145,-5, +-24,-146,-5, +-21,-146,-5, +-19,-147,-5, +-16,-147,-5, +-13,-147,-5, +-11,-146,-5, +-8,-146,-5, +-5,-146,-5, +-3,-147,-5, +0,-147,-5, +3,-147,-5, +6,-147,-5, +9,-147,-5, +12,-147,-5, +15,-147,-5, +18,-147,-5, +20,-148,-5, +23,-148,-5, +26,-148,-5, +29,-148,-5, +32,-148,-5, +35,-148,-5, +38,-148,-5, +41,-148,-5, +44,-148,-5, +47,-148,-5, +49,-149,-5, +52,-149,-5, +55,-149,-5, +58,-149,-5, +61,-149,-5, +64,-149,-5, +67,-149,-5, +70,-149,-5, +73,-149,-5, +76,-149,-5, +78,-150,-5, +81,-150,-5, +84,-150,-5, +86,-149,-5, +88,-148,-5, +90,-147,-5, +93,-147,-5, +95,-146,-5, +97,-145,-5, +99,-144,-5, +102,-144,-5, +104,-143,-5, +107,-143,-5, +109,-142,-5, +112,-142,-5, +114,-141,-5, +116,-140,-5, +119,-140,-5, +121,-139,-5, +123,-138,-5, +126,-138,-5, +128,-137,-5, +130,-136,-5, +133,-136,-5, +135,-135,-5, +137,-134,-5, +140,-134,-5, +142,-133,-5, +144,-132,-5, +147,-132,-5, +149,-131,-5, +151,-130,-5, +154,-130,-5, +156,-129,-5, +159,-129,-5, +161,-128,-5, +163,-127,-5, +166,-127,-5, +168,-126,-5, +170,-125,-5, +173,-125,-5, +175,-124,-5, +177,-123,-5, +180,-123,-5, +182,-122,-5, +184,-121,-5, +187,-121,-5, +189,-120,-5, +191,-119,-5, +194,-119,-5, +196,-118,-5, +199,-117,-5, +202,-117,-5, +204,-116,-5, +206,-115,-5, +209,-115,-5, +211,-114,-5, +213,-113,-5, +216,-113,-5, +218,-112,-5, +220,-111,-5, +221,-109,-5, +222,-107,-5, +222,-104,-5, +223,-101,-5, +224,-99,-5, +225,-97,-5, +226,-95,-5, +227,-93,-5, +228,-91,-5, +229,-89,-5, +230,-87,-5, +231,-85,-5, +232,-83,-5, +233,-81,-5, +233,-78,-5, +234,-76,-5, +235,-74,-5, +236,-72,-5, +237,-70,-5, +238,-68,-5, +239,-66,-5, +240,-64,-5, +241,-62,-5, +242,-60,-5, +243,-58,-5, +244,-56,-5, +245,-54,-5, +246,-52,-5, +246,-49,-5, +247,-47,-5, +248,-45,-5, +249,-43,-5, +250,-41,-5, +251,-39,-5, +252,-37,-5, +253,-35,-5, +254,-32,-5, +255,-29,-5, +257,-26,-5, +258,-23,-5, +259,-20,-5, +261,-17,-5, +262,-14,-5, +258,-16,-5, +256,-19,-5, +254,-20,-5, +252,-21,-5, +250,-23,-5, +248,-24,-5, +246,-25,-5, +245,-27,-5, +243,-28,-5, +241,-29,-5, +240,-31,-5, +238,-32,-5, +236,-33,-5, +235,-35,-5, +233,-36,-5, +231,-37,-5, +229,-38,-5, +228,-40,-5, +226,-41,-5, +224,-42,-5, +223,-44,-5, +221,-45,-5, +219,-46,-5, +218,-48,-5, +216,-49,-5, +214,-50,-5, +212,-51,-5, +211,-53,-5, +209,-54,-5, +207,-55,-5, +206,-57,-5, +204,-58,-5, +202,-59,-5, +201,-61,-5, +199,-62,-5, +197,-63,-5, +196,-65,-5, +194,-66,-5, +192,-67,-5, +190,-68,-5, +189,-70,-5, +187,-71,-5, +185,-72,-5, +184,-74,-5, +182,-75,-5, +180,-76,-5, +179,-78,-5, +177,-79,-5, +175,-80,-5, +174,-82,-5, +172,-83,-5, +170,-84,-5, +168,-86,-5, +166,-87,-5, +165,-89,-5, +163,-90,-5, +161,-91,-5, +159,-93,-5, +157,-94,-5, +156,-96,-5, +154,-97,-5, +152,-98,-5, +150,-99,-5, +148,-100,-5, +146,-99,-5, +144,-98,-5, +142,-97,-5, +140,-96,-5, +138,-95,-5, +136,-94,-5, +134,-93,-5, +131,-93,-5, +129,-92,-5, +126,-92,-5, +124,-91,-5, +122,-90,-5, +119,-90,-5, +117,-89,-5, +115,-88,-5, +113,-87,-5, +110,-87,-5, +108,-86,-5, +106,-85,-5, +103,-85,-5, +101,-84,-5, +99,-83,-5, +97,-83,-5, +96,-82,-5, +94,-82,-5, +92,-82,-5, +91,-81,-5, +89,-81,-5, +88,-80,-5, +86,-80,-5, +85,-79,-5, +83,-79,-5, +82,-78,-5, +80,-78,-5, +79,-77,-5, +77,-77,-5, +76,-76,-5, +74,-76,-5, +72,-75,-5, +70,-75,-5, +69,-74,-5, +67,-74,-5, +66,-73,-5, +64,-73,-5, +63,-72,-5, +61,-72,-5, +59,-72,-5, +58,-71,-5, +56,-71,-5, +55,-70,-5, +53,-70,-5, +52,-69,-5, +50,-69,-5, +49,-68,-5, +47,-68,-5, +46,-67,-5, +44,-67,-5, +43,-66,-5, +41,-66,-5, +40,-65,-5, +38,-65,-5, +36,-64,-5, +34,-64,-5, +33,-63,-5, +31,-63,-5, +30,-62,-5, +28,-62,-5, +27,-61,-5, +25,-61,-5, +23,-61,-5, +22,-60,-5, +20,-60,-5, +19,-59,-5, +17,-59,-5, +16,-58,-5, +14,-58,-5, +13,-57,-5, +11,-57,-5, +10,-56,-5, +8,-56,-5, +7,-55,-5, +5,-55,-5, +4,-56,-5, +3,-57,-5, +2,-58,-5, +1,-59,-5, +1,-61,-5, +0,-62,-5, +-1,-63,-5, +-2,-64,-5, +-3,-65,-5, +-4,-66,-5, +-5,-67,-5, +-6,-68,-5, +-7,-69,-5, +-9,-69,-5, +-10,-70,-5, +-11,-71,-5, +-12,-72,-5, +-13,-73,-5, +-14,-74,-5, +-15,-75,-5, +-16,-76,-5, +-17,-77,-5, +-18,-78,-5, +-19,-79,-5, +-20,-80,-5, +-21,-81,-5, +-22,-82,-5, +-23,-83,-5, +-24,-84,-5, +-26,-85,-5, +-27,-86,-5, +-28,-87,-5, +-29,-88,-5, +-30,-89,-5, +-31,-90,-5, +-32,-91,-5, +-33,-92,-5, +-34,-93,-5, +-36,-93,-5, +-37,-94,-5, +-38,-95,-5, +-39,-96,-5, +-40,-97,-5, +-41,-98,-5, +-42,-99,-5, +-43,-100,-5, +-44,-101,-5, +-45,-102,-5, +-46,-103,-5, +-47,-104,-5, +-48,-105,-5, +-49,-106,-5, +-50,-107,-5, +-51,-108,-5, +-52,-109,-5, +-53,-110,-5, +-54,-111,-5, +-55,-112,-5, +-57,-112,-5, +-58,-113,-5, +-59,-115,-5, +-61,-116,-5, +-62,-118,-5, +-64,-119,-5, +-65,-121,-5, +-67,-122,-5, +-69,-123,-5, +-70,-125,-5, +-72,-126,-5, +-73,-128,-5, +-75,-129,-5, +-77,-131,-5, +-79,-132,-5, +-80,-134,-5, +-82,-135,-5, +-83,-137,-5, +-85,-138,-5, +-86,-140,-5, +-88,-141,-5, +-90,-142,-5, +-91,-144,-5, +-93,-145,-5, +-94,-147,-5, +-96,-148,-5, +-98,-150,-5, +-100,-151,-5, +-101,-153,-5, +-103,-154,-5, +-104,-156,-5, +-106,-157,-5, +-107,-159,-5, +-109,-160,-5, +-111,-161,-5, +-112,-163,-5, +-114,-164,-5, +-115,-166,-5, +-117,-167,-5, +-118,-169,-5, +-120,-170,-5, +-122,-171,-5, +-123,-173,-5, +-125,-174,-5, +-127,-175,-5, +-130,-175,-5, +-132,-176,-5, +-134,-177,-5, +-136,-178,-5, +-138,-179,-5, +-140,-180,-5, +-142,-181,-5, +-144,-182,-5, +-146,-183,-5, +-148,-184,-5, +-150,-185,-5, +-152,-186,-5, +-155,-186,-5, +-157,-187,-5, +-159,-188,-5, +-161,-189,-5, +-163,-190,-5, +-165,-191,-5, +-167,-192,-5, +-170,-193,-5, +-173,-195,-5, +-176,-196,-5, +-179,-198,-5, +-182,-199,-5, +-185,-200,-5, +-188,-202,-5, +-191,-203,-5, +-194,-204,-5, +-197,-206,-5, +-200,-207,-5, +-203,-209,-5, +-206,-210,-5, +-209,-211,-5, +-212,-213,-5, +-215,-214,-5, +-218,-216,-5, +-221,-217,-5, +-224,-218,-5, +-227,-220,-5, +-230,-221,-5, +-233,-222,-5, +-236,-224,-5, +-239,-225,-5, +-242,-227,-5, +-245,-228,-5, +-248,-229,-5, +-251,-231,-5, +-254,-232,-5, +-257,-233,-5, +-260,-235,-5, +-263,-236,-5, +-266,-237,-5, +-269,-238,-5, +-273,-238,-5, +-277,-238,-5, +-280,-239,-5, +-283,-240,-5, +-287,-240,-5, +-290,-241,-5, +-293,-242,-5, +-297,-242,-5, +-300,-243,-5, +-304,-243,-5, +-307,-244,-5, +-311,-244,-5, +-314,-245,-5, +-317,-246,-5, +-321,-246,-5, +-324,-247,-5, +-328,-247,-5, +-331,-248,-5, +-335,-248,-5, +-338,-249,-5, +-341,-250,-5, +-345,-250,-5, +-348,-251,-5, +-352,-251,-5, +-355,-252,-5, +-359,-253,-5, +-363,-253,-5, +-366,-254,-5, +-370,-254,-5, +-374,-254,-5, +-377,-253,-5, +-380,-252,-5, +-383,-251,-5, +-387,-251,-5, +-391,-251,-5, +-394,-250,-5, +-398,-250,-5, +-401,-249,-5, +-405,-249,-5, +-408,-248,-5, +-409,-245,-5, +-408,-242,-5, +-408,-238,-5, +-408,-234,-5, +-409,-231,-5, +-409,-227,-5, +-410,-224,-5, +-410,-220,-5, +-410,-216,-5, +-410,-212,-5, +-411,-209,-5, +-411,-205,-5, +-411,-201,-5, +-412,-198,-5, +-412,-194,-5, +-412,-190,-5, +-412,-186,-5, +-413,-183,-5, +-413,-179,-5, +-413,-175,-5, +-413,-171,-5, +-414,-168,-5, +-414,-164,-5, +-414,-160,-5, +-415,-157,-5, +-415,-153,-5, +-415,-149,-5, +-415,-145,-5, +-416,-142,-5, +-416,-138,-5, +-416,-134,-5, +-416,-130,-5, +-417,-127,-5, +-417,-123,-5, +-417,-119,-5, +-417,-115,-5, +-418,-112,-5, +-418,-108,-5, +-418,-104,-5, +-419,-101,-5, +-419,-97,-5, +-419,-93,-5, +-419,-89,-5, +-420,-86,-5, +-420,-82,-5, +-420,-78,-5, +-420,-74,-5, +-421,-71,-5, +-421,-67,-5, +-421,-63,-5, +-422,-60,-5, +-422,-56,-5, +-423,-53,-5, +-425,-56,-5, +-428,-58,-5, +-431,-60,-5, +-434,-62,-5, +-437,-64,-5, +-440,-65,-5, +-443,-67,-5, +-446,-68,-5, +-449,-70,-5, +-452,-72,-5, +-455,-74,-5, +-458,-76,-5, +-461,-78,-5, +-464,-80,-5, +-467,-82,-5, +-470,-83,-5, +-473,-85,-5, +-476,-87,-5, +-479,-89,-5, +-482,-91,-5, +-485,-93,-5, +-488,-95,-5, +-491,-96,-5, +-494,-98,-5, +-497,-100,-5, +-500,-102,-5, +-504,-105,-5, +-508,-107,-5, +-512,-109,-5, +-515,-112,-5, +-519,-114,-5, +-523,-116,-5, +-526,-119,-5, +-530,-121,-5, +-534,-123,-5, +-538,-126,-5, +-542,-128,-5, +-546,-130,-5, +-549,-133,-5, +-542,-128,-5, +-538,-126,-5, +-534,-124,-5, +-530,-122,-5, +-527,-119,-5, +-523,-117,-5, +-519,-115,-5, +-515,-113,-5, +-512,-110,-5, +-508,-108,-5, +-504,-106,-5, +-501,-104,-5, +-498,-102,-5, +-495,-101,-5, +-492,-99,-5, +-489,-97,-5, +-486,-95,-5, +-483,-93,-5, +-480,-92,-5, +-477,-90,-5, +-474,-88,-5, +-471,-86,-5, +-468,-84,-5, +-465,-83,-5, +-462,-81,-5, +-459,-79,-5, +-456,-77,-5, +-453,-76,-5, +-450,-74,-5, +-447,-72,-5, +-444,-70,-5, +-441,-68,-5, +-438,-67,-5, +-435,-65,-5, +-432,-63,-5, +-429,-61,-5, +-426,-59,-5, +-423,-58,-5, +-420,-56,-5, +-417,-54,-5, +-414,-52,-5, +-411,-50,-5, +-408,-49,-5, +-405,-47,-5, +-402,-45,-5, +-399,-43,-5, +-396,-42,-5, +-393,-41,-5, +-390,-40,-5, +-386,-40,-5, +-382,-40,-5, +-379,-39,-5, +-376,-38,-5, +-373,-37,-5, +-369,-36,-5, +-365,-36,-5, +-362,-35,-5, +-359,-34,-5, +-355,-34,-5, +-352,-33,-5, +-348,-32,-5, +-344,-32,-5, +-341,-31,-5, +-338,-30,-5, +-334,-30,-5, +-331,-29,-5, +-328,-28,-5, +-324,-28,-5, +-321,-27,-5, +-318,-26,-5, +-314,-26,-5, +-311,-25,-5, +-307,-25,-5, +-304,-24,-5, +-301,-23,-5, +-297,-23,-5, +-294,-22,-5, +-291,-21,-5, +-287,-21,-5, +-284,-20,-5, +-281,-19,-5, +-277,-19,-5, +-274,-18,-5, +-271,-17,-5, +-267,-17,-5, +-264,-16,-5, +-260,-16,-5, +-257,-15,-5, +-254,-15,-5, +-251,-15,-5, +-249,-14,-5, +-246,-14,-5, +-243,-14,-5, +-241,-13,-5, +-238,-13,-5, +-236,-12,-5, +-233,-12,-5, +-230,-12,-5, +-228,-11,-5, +-225,-11,-5, +-222,-11,-5, +-220,-10,-5, +-217,-10,-5, +-214,-10,-5, +-212,-9,-5, +-209,-9,-5, +-207,-8,-5, +-204,-8,-5, +-201,-8,-5, +-199,-7,-5, +-196,-7,-5, +-193,-7,-5, +-191,-6,-5, +-188,-6,-5, +-187,-8,-5, +-187,-11,-5, +-187,-14,-5, +-187,-17,-5, +-186,-19,-5, +-186,-22,-5, +-185,-24,-5, +-184,-26,-5, +-183,-28,-5, +-182,-30,-5, +-182,-33,-5, +-181,-35,-5, +-181,-38,-5, +-180,-40,-5, +-180,-43,-5, +-179,-45,-5, +-178,-47,-5, +-178,-50,-5, +-177,-52,-5, +-177,-55,-5, +-176,-57,-5, +-175,-59,-5, +-175,-62,-5, +-174,-64,-5, +-174,-67,-5, +-173,-69,-5, +-172,-71,-5, +-172,-74,-5, +-171,-76,-5, +-171,-79,-5, +-170,-81,-5, +-169,-83,-5, +-169,-86,-5, +-168,-88,-5, +-168,-91,-5, +-167,-93,-5, +-166,-95,-5, +-166,-98,-5, +-165,-100,-5, +-165,-103,-5, +-164,-105,-5, +-163,-107,-5, +-163,-110,-5, +-162,-112,-5, +-162,-115,-5, +-161,-117,-5, +-160,-119,-5, +-160,-122,-5, +-159,-124,-5, +-159,-127,-5, +-158,-129,-5, +-157,-131,-5, +-157,-134,-5, +-156,-136,-5, +-156,-139,-5, +-155,-141,-5, +-154,-143,-5, +-154,-146,-5, +-153,-148,-5, +-153,-151,-5, +-152,-153,-5, +-151,-155,-5, +-151,-158,-5, +-150,-160,-5, +-150,-163,-5, +-149,-165,-5, +-148,-167,-5, +-148,-170,-5, +-147,-172,-5, +-147,-175,-5, +-146,-177,-5, +-145,-179,-5, +-145,-182,-5, +-144,-184,-5, +-144,-187,-5, +-143,-189,-5, +-142,-191,-5, +-142,-194,-5, +-141,-196,-5, +-141,-199,-5, +-140,-201,-5, +-139,-204,-5, +-139,-207,-5, +-138,-209,-5, +-138,-212,-5, +-137,-214,-5, +-136,-216,-5, +-136,-220,-5, +-135,-223,-5, +-134,-226,-5, +-133,-229,-5, +-132,-232,-5, +-132,-236,-5, +-131,-239,-5, +-130,-242,-5, +-129,-245,-5, +-128,-248,-5, +-128,-252,-5, +-127,-255,-5, +-126,-258,-5, +-125,-261,-5, +-124,-264,-5, +-124,-268,-5, +-123,-271,-5, +-122,-274,-5, +-121,-277,-5, +-120,-280,-5, +-120,-284,-5, +-119,-287,-5, +-118,-290,-5, +-117,-293,-5, +-116,-296,-5, +-116,-300,-5, +-115,-303,-5, +-114,-306,-5, +-113,-309,-5, +-112,-312,-5, +-112,-316,-5, +-111,-319,-5, +-110,-322,-5, +-109,-325,-5, +-108,-328,-5, +-108,-332,-5, +-107,-335,-5, +-106,-338,-5, +-106,-342,-5, +-109,-343,-5, +-113,-343,-5, +-116,-344,-5, +-119,-345,-5, +-122,-347,-5, +-125,-349,-5, +-128,-350,-5, +-131,-352,-5, +-134,-353,-5, +-137,-355,-5, +-140,-356,-5, +-143,-357,-5, +-146,-359,-5, +-149,-360,-5, +-152,-361,-5, +-155,-363,-5, +-158,-364,-5, +-161,-366,-5, +-164,-367,-5, +-167,-368,-5, +-170,-370,-5, +-173,-371,-5, +-176,-373,-5, +-179,-374,-5, +-182,-375,-5, +-185,-377,-5, +-188,-378,-5, +-191,-379,-5, +-194,-381,-5, +-197,-382,-5, +-200,-384,-5, +-203,-385,-5, +-206,-386,-5, +-209,-388,-5, +-212,-389,-5, +-215,-391,-5, +-218,-392,-5, +-221,-393,-5, +-224,-395,-5, +-227,-396,-5, +-230,-397,-5, +-233,-399,-5, +-236,-400,-5, +-239,-402,-5, +-242,-403,-5, +-245,-404,-5, +-248,-406,-5, +-251,-407,-5, +-254,-408,-5, +-257,-410,-5, +-260,-411,-5, +-263,-413,-5, +-266,-414,-5, +-269,-415,-5, +-272,-416,-5, +-275,-417,-5, +-278,-418,-5, +-281,-419,-5, +-284,-420,-5, +-287,-421,-5, +-291,-422,-5, +-295,-423,-5, +-299,-425,-5, +-303,-426,-5, +-307,-427,-5, +-311,-429,-5, +-315,-430,-5, +-319,-432,-5, +-323,-433,-5, +-327,-434,-5, +-331,-436,-5, +-335,-437,-5, +-339,-438,-5, +-343,-440,-5, +-347,-441,-5, +-351,-442,-5, +-355,-444,-5, +-359,-445,-5, +-363,-446,-5, +-367,-448,-5, +-371,-449,-5, +-375,-450,-5, +-379,-452,-5, +-383,-453,-5, +-387,-454,-5, +-391,-456,-5, +-395,-457,-5, +-399,-459,-5, +-403,-460,-5, +-407,-461,-5, +-411,-463,-5, +-415,-464,-5, +-419,-465,-5, +-423,-467,-5, +-427,-468,-5, +-431,-469,-5, +-435,-470,-5, +-439,-471,-5, +-444,-471,-5, +-448,-472,-5, +-452,-473,-5, +-456,-474,-5, +-461,-474,-5, +-465,-475,-5, +-469,-476,-5, +-473,-477,-5, +-478,-477,-5, +-482,-478,-5, +-486,-479,-5, +-491,-479,-5, +-495,-480,-5, +-499,-481,-5, +-503,-482,-5, +-508,-482,-5, +-512,-483,-5, +-516,-484,-5, +-520,-485,-5, +-525,-485,-5, +-529,-486,-5, +-533,-487,-5, +-538,-487,-5, +-542,-488,-5, +-546,-489,-5, +-550,-490,-5, +-555,-490,-5, +-559,-491,-5, +-563,-492,-5, +-567,-493,-5, +-572,-493,-5, +-576,-494,-5, +-580,-495,-5, +-585,-495,-5, +-589,-496,-5, +-593,-497,-5, +-597,-498,-5, +-602,-498,-5, +-606,-499,-5, +-610,-500,-5, +-615,-500,-5, +-619,-501,-5, +-623,-502,-5, +-627,-503,-5, +-632,-503,-5, +-636,-504,-5, +-640,-505,-5, +-645,-505,-5, +-649,-504,-5, +-653,-503,-5, +-658,-503,-5, +-663,-503,-5, +-668,-503,-5, +-672,-502,-5, +-677,-502,-5, +-682,-502,-5, +-686,-501,-5, +-691,-501,-5, +-696,-501,-5, +-700,-500,-5, +-705,-500,-5, +-710,-500,-5, +-714,-499,-5, +-719,-499,-5, +-724,-499,-5, +-728,-498,-5, +-733,-498,-5, +-738,-498,-5, +-743,-498,-5, +-748,-498,-5, +-752,-497,-5, +-757,-497,-5, +-762,-497,-5, +-767,-497,-5, +-772,-497,-5, +-777,-497,-5, +-781,-496,-5, +-786,-496,-5, +-791,-496,-5, +-796,-496,-5, +-801,-496,-5, +-805,-495,-5, +-810,-495,-5, +-815,-495,-5, +-820,-495,-5, +-825,-495,-5, +-830,-495,-5, +-834,-494,-5, +-839,-494,-5, +-844,-494,-5, +-849,-494,-5, +-854,-494,-5, +-858,-493,-5, +-863,-493,-5, +-868,-493,-5, +-873,-493,-5, +-878,-493,-5, +-883,-493,-5, +-887,-492,-5, +-892,-492,-5, +-897,-492,-5, +-903,-492,-5, +-909,-492,-5, +-914,-491,-5, +-920,-491,-5, +-926,-491,-5, +-932,-491,-5, +-937,-490,-5, +-943,-490,-5, +-949,-490,-5, +-955,-490,-5, +-961,-490,-5, +-966,-489,-5, +-972,-489,-5, +-978,-489,-5, +-984,-489,-5, +-990,-489,-5, +-995,-488,-5, +-1001,-488,-5, +-1007,-488,-5, +-1013,-488,-5, +-1018,-486,-5, +-1023,-484,-5, +-1028,-482,-5, +-1033,-480,-5, +-1038,-479,-5, +-1043,-477,-5, +-1048,-476,-5, +-1053,-474,-5, +-1058,-472,-5, +-1063,-471,-5, +-1068,-469,-5, +-1073,-467,-5, +-1078,-466,-5, +-1083,-464,-5, +-1088,-462,-5, +-1093,-461,-5, +-1098,-459,-5, +-1103,-457,-5, +-1108,-456,-5, +-1113,-454,-5, +-1118,-452,-5, +-1123,-451,-5, +-1128,-449,-5, +-1133,-447,-5, +-1138,-446,-5, +-1143,-444,-5, +-1148,-442,-5, +-1153,-441,-5, +-1158,-439,-5, +-1163,-437,-5, +-1168,-436,-5, +-1173,-435,-5, +-1179,-435,-5, +-1185,-435,-5, +-1190,-434,-5, +-1196,-434,-5, +-1201,-433,-5, +-1207,-433,-5, +-1213,-433,-5, +-1218,-432,-5, +-1224,-432,-5, +-1230,-432,-5, +-1235,-431,-5, +-1241,-431,-5, +-1247,-431,-5, +-1252,-430,-5, +-1258,-430,-5, +-1264,-430,-5, +-1269,-429,-5, +-1275,-429,-5, +-1281,-429,-5, +-1286,-428,-5, +-1292,-428,-5, +-1298,-428,-5, +-1303,-427,-5, +-1309,-427,-5, +-1315,-427,-5, +-1320,-426,-5, +-1326,-426,-5, +-1332,-426,-5, +-1337,-425,-5, +-1343,-425,-5, +-1349,-425,-5, +-1354,-424,-5, +-1360,-424,-5, +-1366,-424,-5, +-1371,-423,-5, +-1377,-423,-5, +-1382,-422,-5, +-1388,-422,-5, +-1394,-422,-5, +-1399,-421,-5, +-1405,-421,-5, +-1411,-421,-5, +-1416,-420,-5, +-1422,-420,-5, +-1428,-420,-5, +-1433,-419,-5, +-1439,-419,-5, +-1445,-419,-5, +-1450,-418,-5, +-1456,-418,-5, +-1462,-418,-5, +-1467,-417,-5, +-1473,-417,-5, +-1479,-417,-5, +-1484,-416,-5, +-1490,-416,-5, +-1496,-416,-5, +-1501,-415,-5, +-1507,-415,-5, +-1512,-416,-5, +-1517,-417,-5, +-1523,-417,-5, +-1529,-417,-5, +-1534,-418,-5, +-1539,-419,-5, +-1545,-419,-5, +-1550,-420,-5, +-1556,-420,-5, +-1561,-421,-5, +-1567,-421,-5, +-1572,-422,-5, +-1578,-422,-5, +-1583,-423,-5, +-1589,-423,-5, +-1594,-424,-5, +-1600,-424,-5, +-1605,-425,-5, +-1611,-425,-5, +-1616,-426,-5, +-1621,-427,-5, +-1627,-427,-5, +-1632,-428,-5, +-1638,-428,-5, +-1643,-429,-5, +-1649,-429,-5, +-1654,-430,-5, +-1660,-430,-5, +-1665,-431,-5, +-1671,-431,-5, +-1676,-432,-5, +-1682,-432,-5, +-1687,-433,-5, +-1692,-434,-5, +-1695,-439,-5, +-1698,-444,-5, +-1702,-448,-5, +-1706,-452,-5, +-1710,-456,-5, +-1714,-460,-5, +-1718,-464,-5, +-1721,-469,-5, +-1725,-473,-5, +-1729,-477,-5, +-1733,-481,-5, +-1737,-485,-5, +-1740,-490,-5, +-1744,-494,-5, +-1748,-498,-5, +-1752,-502,-5, +-1755,-507,-5, +-1759,-511,-5, +-1763,-515,-5, +-1768,-517,-5, +-1774,-517,-5, +-1780,-517,-5, +-1785,-518,-5, +-1790,-520,-5, +-1796,-520,-5, +-1801,-521,-5, +-1806,-522,-5, +-1811,-523,-5, +-1816,-524,-5, +-1821,-525,-5, +-1826,-526,-5, +-1832,-526,-5, +-1837,-527,-5, +-1842,-528,-5, +-1847,-529,-5, +-1852,-530,-5, +-1857,-531,-5, +-1863,-531,-5, +-1868,-532,-5, +-1873,-533,-5, +-1878,-534,-5, +-1883,-535,-5, +-1888,-536,-5, +-1894,-536,-5, +-1899,-537,-5, +-1904,-538,-5, +-1909,-539,-5, +-1914,-540,-5, +-1919,-541,-5, +-1924,-542,-5, +-1930,-542,-5, +-1935,-543,-5, +-1940,-544,-5, +-1945,-545,-5, +-1950,-546,-5, +-1955,-547,-5, +-1960,-548,-5, +-1966,-548,-5, +-1971,-549,-5, +-1977,-550,-5, +-1983,-551,-5, +-1989,-552,-5, +-1995,-553,-5, +-2001,-554,-5, +-2007,-555,-5, +-2013,-556,-5, +-2019,-557,-5, +-2025,-558,-5, +-2031,-559,-5, +-2037,-560,-5, +-2043,-561,-5, +-2049,-562,-5, +-2055,-563,-5, +-2059,-558,-5, +-2063,-553,-5, +-2068,-549,-5, +-2073,-545,-5, +-2077,-540,-5, +-2081,-535,-5, +-2086,-531,-5, +-2091,-527,-5, +-2095,-522,-5, +-2100,-518,-5, +-2104,-513,-5, +-2109,-509,-5, +-2114,-504,-5, +-2119,-500,-5, +-2123,-495,-5, +-2128,-491,-5, +-2132,-486,-5, +-2137,-482,-5, +-2141,-477,-5, +-2146,-473,-5, +-2150,-468,-5, +-2155,-464,-5, +-2160,-460,-5, +-2164,-455,-5, +-2169,-451,-5, +-2173,-446,-5, +-2178,-442,-5, +-2182,-437,-5, +-2187,-433,-5, +-2192,-428,-5, +-2197,-424,-5, +-2201,-419,-5, +-2206,-415,-5, +-2210,-410,-5, +-2215,-406,-5, +-2219,-401,-5, +-2224,-397,-5, +-2228,-392,-5, diff --git a/src/modules/navigator/navigator_tests/navigator_tests.cpp b/src/modules/navigator/navigator_tests/navigator_tests.cpp new file mode 100644 index 0000000000..fb39b26dac --- /dev/null +++ b/src/modules/navigator/navigator_tests/navigator_tests.cpp @@ -0,0 +1,15 @@ + +/** + * Navigator unit tests. Run the tests as follows: + * nsh> navigator_tests + * + */ + +//#include + +#include "tracker_test.h" + +extern "C" __EXPORT int navigator_tests_main(int argc, char *argv[]) +{ + return trackerTest() ? 0 : -1; +} diff --git a/src/modules/navigator/navigator_tests/no_loop.path b/src/modules/navigator/navigator_tests/no_loop.path new file mode 100644 index 0000000000..1315fd2cb4 --- /dev/null +++ b/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, diff --git a/src/modules/navigator/navigator_tests/simple_jumps.path b/src/modules/navigator/navigator_tests/simple_jumps.path new file mode 100644 index 0000000000..efe4d2ca2e --- /dev/null +++ b/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, diff --git a/src/modules/navigator/navigator_tests/simple_loop.path b/src/modules/navigator/navigator_tests/simple_loop.path new file mode 100644 index 0000000000..7410c5871e --- /dev/null +++ b/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, diff --git a/src/modules/navigator/navigator_tests/tracker_test.cpp b/src/modules/navigator/navigator_tests/tracker_test.cpp new file mode 100644 index 0000000000..0eea5ec099 --- /dev/null +++ b/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 // sqrt +#include // std::strcat +#include // INT_MAX +#include // 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) diff --git a/src/modules/navigator/navigator_tests/tracker_test.h b/src/modules/navigator/navigator_tests/tracker_test.h new file mode 100644 index 0000000000..d8ec437eee --- /dev/null +++ b/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(); diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index fa097b6860..7789540490 100644 --- a/src/modules/navigator/rtl_params.c +++ b/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); + diff --git a/src/modules/navigator/smart_rtl.cpp b/src/modules/navigator/smart_rtl.cpp new file mode 100644 index 0000000000..5b7852311a --- /dev/null +++ b/src/modules/navigator/smart_rtl.cpp @@ -0,0 +1,220 @@ +/** + * @file rcrecover.cpp + * RC recovery navigation mode + */ + +#include "smart_rtl.h" + +#include +#include +#include + +#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); +} diff --git a/src/modules/navigator/smart_rtl.h b/src/modules/navigator/smart_rtl.h new file mode 100644 index 0000000000..601487a62c --- /dev/null +++ b/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 + +#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) _param_rtl_fallbck_dly, + (ParamFloat) _param_rtl_land_delay + ) +}; diff --git a/src/modules/navigator/tracker.cpp b/src/modules/navigator/tracker.cpp new file mode 100644 index 0000000000..9115e7b044 --- /dev/null +++ b/src/modules/navigator/tracker.cpp @@ -0,0 +1,1916 @@ +/** + * @file tracker.cpp + * + * Tracks the flight graph in a memory optimized format. + * See tracker.h for an overview. + * + * @author Samuel Sadok + */ + +#include "tracker.h" + +#include +#include +#include + +#include +#include +#include + +template void myswap(T &a, T &b) +{ + T temp(a); + a = b; + b = temp; +} + +float Tracker::fast_sqrt(uint16_t val, bool fallback_to_infinity) +{ + const float values[] = { + 0.000000000000000f, 1.000000000000000f, 1.414213562373095f, 1.732050807568877f, + 2.000000000000000f, 2.236067977499790f, 2.449489742783178f, 2.645751311064591f, + 2.828427124746190f, 3.000000000000000f, 3.162277660168380f, 3.316624790355400f, + 3.464101615137754f, 3.605551275463989f, 3.741657386773941f, 3.872983346207417f, + 4.000000000000000f, 4.123105625617661f, 4.242640687119285f, 4.358898943540674f, + 4.472135954999580f, 4.582575694955840f, 4.690415759823430f, 4.795831523312719f, + 4.898979485566356f, 5.000000000000000f, 5.099019513592784f, 5.196152422706632f, + 5.291502622129181f, 5.385164807134504f, 5.477225575051661f, 5.567764362830022f + }; + + if (val < sizeof(values) / sizeof(values[0])) { + return values[val]; + } + + if (fallback_to_infinity) { + return INFINITY; + } + + float result = sqrtf(val); + return result; +} + +int Tracker::dot(ipos_t vec1, ipos_t vec2) +{ + if (!fits_into_far_delta(vec1) || !fits_into_far_delta(vec2)) { + return INT_MAX; + } + + return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z; +} + +Tracker::ipos_t Tracker::get_point_to_line_delta(ipos_t point, ipos_t line_delta, ipos_t line_end, int &coef) +{ + point -= line_end; + + // The negative coefficient is obtained by projecting the point along the line direction + coef = float_to_coef(-dot(point, line_delta) / (float)dot(line_delta)); + + // Constrain coefficient to beginning or end of line + int max_coefficient = MAX_COEFFICIENT; + coef = math::max(0, math::min(max_coefficient, coef)); + + //ipos_t result = -apply_coef(line_delta, coef) - point; + //TRACKER_DBG("projecting (%d %d %d) to (delta %d %d %d end %d %d %d) gives coef %d, which gives (%d %d %d)", point.x, point.y, point.z, + // line_delta.x, line_delta.y, line_delta.z, + // line_end.x, line_end.y, line_end.z, + // coef, + // result.x, result.y, result.z); + + // Return result + return -apply_coef(line_delta, coef) - point; +} + +Tracker::ipos_t Tracker::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) +{ + // If either of the two coefficients is pinned, apply it to the line end + if (pin_coef1) { + end1 -= apply_coef(delta1, coef1); + } + + if (pin_coef2) { + end2 -= apply_coef(delta2, coef2); + } + + if (pin_coef1 && pin_coef2) { + return end2 - end1; + } + + // A point-to-line distance is sufficient if a coefficient is pinned. + if (pin_coef1) { + return get_point_to_line_delta(end1, delta2, end2, coef2); + } + + if (pin_coef2) { + return -get_point_to_line_delta(end2, delta1, end1, coef1); + } + + // None of the coefficients are pinned, so we go on to calculate the true line-to-line delta + + // Find the direction that is normal to both lines (using cross product) + ipos_t normal = { + .x = delta1.y * delta2.z - delta1.z * delta2.y, + .y = delta1.z * delta2.x - delta1.x * delta2.z, + .z = delta1.x * delta2.y - delta1.y * delta2.x + }; + + // Project a connection between both lines along the direction of the normal. + ipos_t end_to_end_delta = end2 - end1; + fpos_t temp_delta = to_fpos(normal) * (dot(end_to_end_delta, normal) / (float)dot(normal)); + + // Remove the orthogonal component - the end-to-end delta is now a linear combination of both line directions + end_to_end_delta -= to_ipos(temp_delta); + + // Now we have the following system of equations: + // (-delta1 delta2) * (coef1 coef2)^T = end_to_end_delta + // + // This is over-determined, so what we solve instead is: + // (coef1 coef2)^T = ((-delta1 delta2)^T * (-delta1 delta2))^-1 * (-delta1 delta2)^T * end_to_end_delta + // \---------------- A ----------------/ \---------------- b ----------------/ + // + // A = | dot(-deltaA, -deltaA) dot(-deltaA, deltaB) | + // | dot(-deltaA, deltaB) dot(deltaB, deltaB) | + + // Precalculate A and b + int delta1delta1 = dot(delta1, delta1); + int delta1delta2 = dot(delta1, delta2); + int delta2delta2 = dot(delta2, delta2); + int b1 = -dot(delta1, end_to_end_delta); + int b2 = dot(delta2, end_to_end_delta); + + // If rounding temp_delta is not desired, use this (with unmodified end_to_end_delta!): + //temp_delta = to_fpos(end_to_end_delta) - temp_delta; + //float b1 = -(delta1.x * temp_delta.x + delta1.y * temp_delta.y + delta1.z * temp_delta.z); + //float b2 = (delta2.x * temp_delta.x + delta2.y * temp_delta.y + delta2.z * temp_delta.z); + + // Prepare inverse of A + float determinant = delta1delta1 * delta2delta2 - delta1delta2 * delta1delta2; + + // Calculate coefficients and convert to integers + coef1 = float_to_coef((delta2delta2 * b1 + delta1delta2 * b2) / determinant); + coef2 = float_to_coef((delta1delta2 * b1 + delta1delta1 * b2) / determinant); + + // If both coefficients are within the representable range, we can use this as the result + if (coef1 >= 0 && coef1 <= MAX_COEFFICIENT && coef2 >= 0 && coef2 <= MAX_COEFFICIENT) { + end1 -= apply_coef(delta1, coef1); + end2 -= apply_coef(delta2, coef2); + + // The result should theoretically be equal to temp_delta. + // In practice, we will probably have a different discretization error when applying the coefficients. + // We want to be sure here that the delta is really what is obtained by applying the returned coefficient. + return end2 - end1; + } + + + // If a coefficient is out of reach, we need return one out of four possible point-to-line distances. + // There are probably optimizations we could apply to this, but it's probably not neccessary. + + // Calculate the delta from each point to the other line. + int temp_coef1, temp_coef2; + ipos_t start1 = end1 - apply_coef(delta1, MAX_COEFFICIENT); + ipos_t start2 = end2 - apply_coef(delta2, MAX_COEFFICIENT); + ipos_t start1_to_line2 = get_point_to_line_delta(start1, delta2, end2, temp_coef2); + ipos_t start2_to_line1 = get_point_to_line_delta(start2, delta1, end1, temp_coef1); + ipos_t end1_to_line2 = get_point_to_line_delta(end1, delta2, end2, coef2); + ipos_t end2_to_line1 = get_point_to_line_delta(end2, delta1, end1, coef1); + + //TRACKER_DBG("delta1: (%d %d %d), delta2: (%d %d %d), delta3: (%d %d %d), delta4: (%d %d %d)", + // start1_to_line2.x, start1_to_line2.y, start1_to_line2.z, + // end1_to_line2.x, end1_to_line2.y, end1_to_line2.z, + // start2_to_line1.x, start2_to_line1.y, start2_to_line1.z, + // end2_to_line1.x, end2_to_line1.y, end2_to_line1.z); + + // Of all the point-to-line deltas, return the smallest one. + int best_to_line1 = math::min(dot(start2_to_line1), dot(end2_to_line1)); + int best_to_line2 = math::min(dot(start1_to_line2), dot(end1_to_line2)); + + if (best_to_line1 < best_to_line2) { + if (dot(start2_to_line1) <= best_to_line1) { + coef1 = temp_coef1; + coef2 = MAX_COEFFICIENT; + return -start2_to_line1; + + } else { // end2_to_line1 is the shortest delta + coef2 = 0; + return -end2_to_line1; + } + + } else { + if (dot(start1_to_line2) <= best_to_line2) { + coef1 = MAX_COEFFICIENT; + coef2 = temp_coef2; + return start1_to_line2; + + } else { // end1_to_line2 is the shortest delta + coef1 = 0; + return end1_to_line2; + } + } +} + +Tracker::delta_item_t Tracker::pack_compact_delta_item(ipos_t delta) +{ + return ((delta.x & 0x1F) << 10) | ((delta.y & 0x1F) << 5) | ((delta.z & 0x1F) << 0); +} + +Tracker::ipos_t Tracker::unpack_compact_delta_item(delta_item_t delta) +{ + static constexpr int SHIFT_COUNT = CHAR_BIT * sizeof(int) - 5; + + return { + .x = ((int)(delta << (SHIFT_COUNT - 10))) >> SHIFT_COUNT, + .y = ((int)(delta << (SHIFT_COUNT - 5))) >> SHIFT_COUNT, + .z = ((int)(delta << (SHIFT_COUNT - 0))) >> SHIFT_COUNT + }; +} + +bool Tracker::fits_into_far_delta(ipos_t vec) +{ + return (((vec.x >> 14) == 0) || ((vec.x >> 14) == -1)) && + (((vec.y >> 14) == 0) || ((vec.y >> 14) == -1)) && + (((vec.z >> 14) == 0) || ((vec.z >> 14) == -1)); +} + +bool Tracker::push_delta(size_t &index, ipos_t delta, bool jump, ssize_t max_space, delta_item_t *buffer) +{ + int max_val = math::max(math::max(delta.x, delta.y), delta.z); + int min_val = math::min(math::min(delta.x, delta.y), delta.z); + + // If the delta is too large for a single compact delta item, we split it into multiple items + int split_count = 1; + + if (max_val > COMPACT_DELTA_MAX || min_val < COMPACT_DELTA_MIN) { + split_count = (int)ceil(math::max((float)max_val / (float)COMPACT_DELTA_MAX, + (float)min_val / (float)COMPACT_DELTA_MIN)); + } + + int far_delta_size = FAR_DELTA_SIZE; + split_count = jump ? far_delta_size : math::min(split_count, far_delta_size); + + if (max_space < split_count) { + return false; + } + + if (split_count >= FAR_DELTA_SIZE || jump) { + if (max_val > FAR_DELTA_MAX || min_val < FAR_DELTA_MIN) { + PX4_ERR("far delta overflow"); + return false; + } + + buffer[index++] = (delta.x & 0x7FFF) | 0x8000; + buffer[index++] = (delta.y & 0x7FFF) | (jump ? 0x8000 : 0); + buffer[index++] = (delta.z & 0x7FFF) | 0x8000; + + } else { + + ipos_t compact_delta_sum = { .x = 0, .y = 0, .z = 0 }; + + for (int i = 1; i < split_count; i++) { + ipos_t compact_delta = to_ipos(to_fpos(delta) * ((float)i / (float)split_count)) - compact_delta_sum; + buffer[index++] = pack_compact_delta_item(compact_delta); + compact_delta_sum += compact_delta; + } + + delta -= compact_delta_sum; + + buffer[index++] = pack_compact_delta_item(delta); + } + + return true; +} + +Tracker::ipos_t Tracker::fetch_delta(size_t index, bool &is_jump, delta_item_t *buffer) +{ + const int SHIFT_COUNT = CHAR_BIT * sizeof(int) - 15; + + if (!((buffer[index] >> 15) & 1)) { + is_jump = false; + return unpack_compact_delta_item(buffer[index]); + } + + //if (buffer[index] == OBSOLETE_DELTA || buffer[index - 1] == OBSOLETE_DELTA || buffer[index - 2] == OBSOLETE_DELTA) + //GRAPH_ERR("fetching invalid delta element"); + + is_jump = (buffer[index - 1] >> 15) & 1; + return { + .x = ((int)((buffer[index - 2] & 0x7FFF) << SHIFT_COUNT)) >> SHIFT_COUNT, + .y = ((int)((buffer[index - 1] & 0x7FFF) << SHIFT_COUNT)) >> SHIFT_COUNT, + .z = ((int)((buffer[index - 0] & 0x7FFF) << SHIFT_COUNT)) >> SHIFT_COUNT + }; +} + +int Tracker::get_granularity_at(ipos_t pos) +{ + int dist_squared = dot(home_position - pos); + + // Calculate max(0, floor(log2(dist_squared))): 0->0, 1->0, 2->1, 3->1, 4->2, 7->2, 8->3, ... + int log_dist = 0; + + while (dist_squared >>= 1) { + log_dist++; + } + + log_dist = (log_dist - HIGH_PRECISION_RANGE) >> 1; + + // The graph precision is lowered every second time the memory pressure increases + int p = (memory_pressure >> 1) + 1; + + int margin = math::max(((p - 1) >> 1) + 1, log_dist * p); + // \___ regime 1 ___/ \ regime 2 / + + // Regime 1: the margin close to home shall increase every second time we reduce precision + // Regime 2: the margin further from home shall increase proportionally to log(distance), where the slope is proportional to pressure + // Everything at distance 2^(HIGH_PRECISION_RANGE/2+1) or closer falls into regime 1 + + return margin * margin; +} + +void Tracker::set_home(float x, float y, float z) +{ + home_position = to_ipos({ .x = x, .y = y, .z = z }); + + // Reset the distance-to-home of all nodes to infinity (except the home node) + for (size_t i = 1; i < node_count; i++) { + node_at(0).distance = MAX_DISTANCE; + } + + node_t &home_node = node_at(0); + home_node.index1 = 0; + home_node.index2 = 0; + home_node.coef1 = 0; + home_node.coef2 = 0; + home_node.dirty = 1; + home_node.obsolete = 0; + home_node.use_line2 = 0; + home_node.go_forward = 0; + home_node.distance = 0; + + consolidate_graph("new home position"); + + did_set_home = true; + + TRACKER_DBG("tracker received new home position %zu-%.2f (%.2f, %.2f, %.2f)", nodes->index1, + (double)coef_to_float(nodes->coef1), (double)x, (double)y, (double)z); +} + +void Tracker::update(vehicle_local_position_s *position) +{ + if (_ref_timestamp == 0 && position->ref_timestamp != 0) { + map_projection_init(&_ref_pos, position->ref_lat, position->ref_lon); + _ref_alt = position->ref_alt; + _ref_timestamp = position->ref_timestamp; + + } else if (_ref_timestamp != position->ref_timestamp) { + // todo: implement changing reference + return; + } + + if (position->xy_valid && position->z_valid && _ref_timestamp != 0) { + update(position->x, position->y, position->z); + } +} + +void Tracker::update(float x, float y, float z) +{ + last_known_position = { + .x = x, + .y = y, + .z = z + }; + + if (recent_path_tracking_enabled) { + push_recent_path(last_known_position); + } + + if (graph_tracking_enabled) { + push_graph(last_known_position); + } + + if (!did_set_home) { + set_home(x, y, z); + } +} + +void Tracker::push_recent_path(fpos_t &position) +{ + ipos_t new_pos = to_ipos(position); + + bool rollback = false; + size_t index = recent_path_next_write; + ipos_t head = recent_path_head; + + if (recent_path_next_read != RECENT_PATH_LENGTH) { + + // If we're still close to the most recent position, don't update + if (dot(head, new_pos) <= 1) { + return; + } + + // Scan the buffer for a close position, starting at the latest one + do { + index = (index ? index : RECENT_PATH_LENGTH) - 1; + + if ((recent_path[index] >> 15) & 1) { + // If this item is a bumper, we shouldn't roll over this invalid position (todo: use far jump element as bumper) + break; + } + + head -= unpack_compact_delta_item(recent_path[index]); + rollback = dot(head, new_pos) <= 1; + } while ((index != recent_path_next_read) && !rollback); + + // If the preceding item would be a bumper, we shouldn't roll back to this invalid position + if (rollback && index != recent_path_next_read) { + if ((recent_path[(index ? index : RECENT_PATH_LENGTH) - 1] >> 15) & 1) { + rollback = false; + } + } + } + + if (rollback) { + // If there was a close position in the path, roll the path back to that position + recent_path_head = head; + + if (index == recent_path_next_write) { + TRACKER_DBG("recent path complete rollback"); + recent_path_next_write = 0; + recent_path_next_read = RECENT_PATH_LENGTH; + + } else { + TRACKER_DBG("recent path rollback to %zu", index); + recent_path_next_write = index; + } + + } else { + // If there was no close position in the path, add the current position to the buffer + // todo: handle large distances + ipos_t old_head = recent_path_head; + + delta_item_t item = pack_compact_delta_item(new_pos - old_head); + + recent_path_head = new_pos; + recent_path[recent_path_next_write] = item; + + if (recent_path_next_write++ == recent_path_next_read) { + recent_path_next_read = recent_path_next_write; + } + + if (recent_path_next_write == RECENT_PATH_LENGTH) { + recent_path_next_write = 0; + } + + if (recent_path_next_read == RECENT_PATH_LENGTH) { + recent_path_next_read = 0; + } + } +} + +bool Tracker::pop_recent_path(fpos_t &position) +{ + position = to_fpos(recent_path_head); + + // Check if path is empty + if (recent_path_next_read == RECENT_PATH_LENGTH) { + return false; + } + + int last_index = (recent_path_next_write ? recent_path_next_write : RECENT_PATH_LENGTH) - 1; + + // Don't consume bumper item + if ((recent_path[last_index] >> 15) & 1) { + return false; + } + + // Roll back most recent position + recent_path_head -= unpack_compact_delta_item(recent_path[last_index]); + + if ((recent_path_next_write = last_index) == recent_path_next_read) { + recent_path_next_write = 0; + recent_path_next_read = RECENT_PATH_LENGTH; + } + + return true; +} + +bool Tracker::pop_recent_path(double &lat, double &lon, float &alt) +{ + fpos_t position; + + if (!pop_recent_path(position)) { + return false; + } + + map_projection_reproject(&_ref_pos, position.x, position.y, &lat, &lon); + alt = _ref_alt - position.z; + return true; +} + +void Tracker::push_graph(fpos_t &position) +{ + ipos_t new_pos = to_ipos(position); + + // Don't update if we're still close to the last known line + if (graph_next_write) { + int coef; + bool is_jump; + ipos_t delta = get_point_to_line_delta(new_pos, fetch_delta(graph_next_write - 1, is_jump), graph_head_pos, coef); + + if (!is_jump && dot(delta) <= get_granularity_at(new_pos)) { + return; + } + } + + TRACKER_DBG("tracker got pos %d %d %d", new_pos.x, new_pos.y, new_pos.z); + + // While there is not enough space left, increase memory pressure (at most 5 attempts) + for (int i = 0; get_free_graph_space() < FAR_DELTA_SIZE * sizeof(delta_item_t); i++) { + if (i >= 5 || memory_pressure == INT_MAX) { + GRAPH_ERR("increasing memory pressure %d times (to %d) did not help", i, memory_pressure); + return; + } + + PX4_WARN("flight graph reached limit at memory pressure %d - flight graph will be optimized", memory_pressure); + + compress_perf_t *perf = memory_pressure > MAX_PERF_MEASUREMENTS ? nullptr : &(perf_measurements[memory_pressure - 1] = { + .runtime = 0, + .deltas_before = graph_next_write, + .nodes_before = node_count + }); + + memory_pressure++; + + size_t free_space_before = get_free_graph_space(); + + hrt_abstime start_time = hrt_absolute_time(); + + // Rewrite graph every other time the memory limit is reached + if (memory_pressure & 1) { + rewrite_graph(); // the rewrite pass includes consolidation + + } else { + consolidated_head_pos = graph_start; + consolidated_head_index = 0; + consolidate_graph("reached memory limit"); + } + + start_time = hrt_absolute_time() - start_time; + //TRACKER_DBG("time: %f", start_time / 1e6f); + + if (perf) { + perf->runtime = start_time; + perf->deltas_after = graph_next_write; + perf->nodes_after = node_count; + }; + + (void)free_space_before; + + TRACKER_DBG("could reduce graph density from %.3f%% to %.3f%% in %f ms", + (double)100 * ((double)1 - (double)free_space_before / (GRAPH_LENGTH * sizeof(delta_item_t))), + (double)100 * ((double)1 - (double)get_free_graph_space() / (GRAPH_LENGTH * sizeof(delta_item_t))), + (double)(start_time / 1e3f)); + } + + if (!graph_next_write) { + graph_head_pos = new_pos; // make sure that the very first delta is a compact delta + graph_start = new_pos; + consolidated_head_pos = new_pos; + consolidated_head_index = 0; + } + + push_delta(graph_next_write, new_pos - graph_head_pos, false); + graph_head_pos = new_pos; + + if (graph_next_write - 1 - consolidated_head_index >= MAX_CONSOLIDATION_DEBT) { + consolidate_graph("consolidation debt limit reached"); + } +} + +Tracker::ipos_t Tracker::walk_forward(size_t &index, bool &is_jump) +{ + if ((graph[index + 1] >> 15) & 1) { + return fetch_delta(index += FAR_DELTA_SIZE, is_jump); + + } else { + return fetch_delta(index += 1, is_jump); + } +} + +Tracker::ipos_t Tracker::walk_backward(size_t &index, bool &is_jump) +{ + ipos_t delta = fetch_delta(index, is_jump); + + if ((graph[index] >> 15) & 1) { + index -= FAR_DELTA_SIZE; + + } else { + index -= 1; + } + + return delta; +} + +bool Tracker::push_node(node_t &node, int granularity) +{ + if (get_free_graph_space() < sizeof(node_t)) { + return false; + } + + float half_accuracy = fast_sqrt(granularity, false); + + // Check if there is already a nearby node that connects roughly the same lines + for (size_t i = node_count - 1; i > 0; i--) { + if (check_similarity(node.index1, node.coef1, node_at(i).index1, node_at(i).coef1, half_accuracy)) { + if (check_similarity(node.index2, node.coef2, node_at(i).index2, node_at(i).coef2, half_accuracy)) { + return false; + } + } + + if (check_similarity(node.index1, node.coef1, node_at(i).index2, node_at(i).coef2, half_accuracy)) { + if (check_similarity(node.index2, node.coef2, node_at(i).index1, node_at(i).coef1, half_accuracy)) { + return false; + } + } + } + + node_at(node_count++) = node; + have_dirty_nodes = true; + return true; // todo: take some action if the buffer becomes full +} + + +void Tracker::remove_nodes(size_t lower_bound, size_t upper_bound) +{ + for (size_t i = 1; i < node_count; i++) { + if ((lower_bound < node_at(i).index1 && node_at(i).index1 <= upper_bound) || + (lower_bound < node_at(i).index2 && node_at(i).index2 <= upper_bound) || + node_at(i).obsolete) { + // move preceding part of the stack + node_count--; + memmove(nodes - node_count + 1, nodes - node_count, (node_count - i) * sizeof(node_t)); + i--; + } + } +} + +bool Tracker::check_similarity(size_t index1, int coef1, size_t index2, int coef2, float max_distance) +{ + if (index1 > index2) { + myswap(index1, index2); + myswap(coef1, coef2); + } + + if (index1 == index2 && coef1 == coef2) { + return true; + } + + float fcoef1 = coef_to_float(coef1); + float fcoef2 = coef_to_float(coef2); + + bool is_jump; + size_t index2_predecessor = index2; + float delta_length = fast_sqrt(dot(walk_backward(index2_predecessor, is_jump)), false); + + if (is_jump && coef2 != MAX_COEFFICIENT) { + return false; + } + + if (index1 == index2) { + if (delta_length * fabsf(fcoef1 - fcoef2) <= max_distance) { + return true; + } + } + + float predecessor_delta_length = fast_sqrt(dot(fetch_delta(index2_predecessor, is_jump)), false); + + if (is_jump && coef1) { + return false; + } + + if (index1 == index2_predecessor) { + if (delta_length * (1 - fcoef2) + predecessor_delta_length * fcoef1 <= max_distance) { + return true; + } + } + + return false; +} + +bool Tracker::is_close_to_graph(ipos_t position, size_t lower_bound, size_t upper_bound, ipos_t pos_at_upper_bound) +{ + int granularity = get_granularity_at(position); + + while (upper_bound > lower_bound && !graph_fault) { + bool is_jump; + ipos_t delta = walk_backward(upper_bound, is_jump); + + int coef; + ipos_t pos_to_line = is_jump ? (position - pos_at_upper_bound) : get_point_to_line_delta(position, delta, + pos_at_upper_bound, coef); + + if (dot(pos_to_line) <= granularity) { + return true; + } + + pos_at_upper_bound -= delta; + } + + return false; +} + +Tracker::ipos_t Tracker::get_closest_position(ipos_t position, size_t &best_index, unsigned int &best_coef) +{ + int best_dist_squared = INT_MAX; + ipos_t best_pos = { .x = 0, .y = 0, .z = 0 }; + best_index = 0; + best_coef = 0; + + if (!graph_next_write) { + return best_pos; + } + + ipos_t pos = graph_head_pos; + size_t index = graph_next_write - 1; + size_t prev_index; + int coef; + + do { + prev_index = index; + bool is_jump; + ipos_t delta = walk_backward(index, is_jump); + + int dummy = 0; + ipos_t line_to_pos = get_line_to_line_delta(delta, pos, { .x = 0, .y = 0, .z = 0 }, position, coef = 0, dummy, is_jump + || !prev_index, true); + int distance_squared = dot(line_to_pos); + + if (best_dist_squared >= distance_squared) { + best_dist_squared = distance_squared; + best_pos = position - line_to_pos; + best_index = prev_index; + best_coef = coef; + } + + pos -= delta; + } while (prev_index && !graph_fault); + + return best_pos; +} + +bool Tracker::is_line(ipos_t start_pos, size_t start_index, ipos_t end_pos, size_t end_index, bool should_be_jump) +{ + ipos_t line_delta = end_pos - start_pos; + ipos_t pos = end_pos; + + // The efficiency gain of prefetching the granularity seems to outweigh the corner-cases there the granularity along the line should be finer than on both ends. + int granularity = math::min(get_granularity_at(start_pos), get_granularity_at(end_pos)); + + // Intuitively, it seems that walking backward allows us to break earlier than walking forward. + + for (;;) { + bool is_jump; + pos -= walk_backward(end_index, is_jump); + + if (graph_fault) { + return false; + } + + // The line must have a uniform jump property. + if (is_jump != should_be_jump) { + return false; + } + + if (end_index <= start_index) { + return true; + } + + int coef; + ipos_t delta = get_point_to_line_delta(pos, line_delta, end_pos, coef); + + if (dot(delta) > granularity) { + return false; + } + } +} + +void Tracker::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) +{ + ipos_t next_end_pos = start_pos; + size_t next_end_index = start_index; + bool next_is_jump = is_jump; + + do { + end_index = next_end_index; + end_pos = next_end_pos; + is_jump = next_is_jump; + next_end_pos += walk_forward(next_end_index, next_is_jump); + } while (!graph_fault && next_end_index <= bound + && is_line(start_pos, start_index, next_end_pos, next_end_index, next_is_jump)); +} + +// Consolidates the most recent positions on the graph, by applying some optimizations: +// 1. Consecutive positions that lie roughly on a line are aggregated. +// 2. If multiple consecutive positions lie close to the graph, they are removed and replaced by a jump. +// 3. For lines that pass close to each other, a node is created. +// The entire tracker is guaranteed to remain consistent, however exposed indices may become invalid. +void Tracker::consolidate_graph(const char *reason) +{ + // Abort if the graph is empty + if (!graph_next_write) { + return; + } + + TRACKER_DBG("consolidating graph from %zu to %zu: %s", consolidated_head_index, graph_next_write - 1, reason); + DUMP_GRAPH(); + + // Remove any nodes that refer to the area that will be modified. + // Any nodes for that area will be newly created anyway. + remove_nodes(consolidated_head_index, graph_next_write - 1); + + /*** Pass 1: detect lines ***/ + TRACKER_DBG(" line detection..."); + + ipos_t pos = consolidated_head_pos; + size_t read_index = consolidated_head_index; + size_t write_index = read_index + 1; + + // read_index and (write_index - 1) always correspond to the same position. + // They start out to be equal, and the gap between them increases as consolidation moves on. + + while (read_index < graph_next_write - 1 && !graph_fault) { + bool is_jump; + ipos_t line_end_pos; + size_t line_end_index; + get_longest_line(pos, read_index, line_end_pos, line_end_index, is_jump, graph_next_write - 1); + + // Push the line delta ONLY if it uses less space than the original deltas + if (push_delta(write_index, line_end_pos - pos, is_jump, line_end_index - read_index)) { + TRACKER_DBG(" aggregated %zu to %zu into line %zu", read_index, line_end_index, write_index - 1); + //TRACKER_DBG(" from %d %d %d to %d %d %d, %s", pos.x, pos.y, pos.z, line_end_pos.x, line_end_pos.y, line_end_pos.z, is_jump ? "is jump" : "no jump"); + read_index = line_end_index; + } + + // Copy the deltas that were not aggregated into a line + while (read_index < line_end_index) { + push_delta(write_index, walk_forward(read_index, is_jump), is_jump); + } + + pos = line_end_pos; + } + + graph_next_write = write_index; + + + /*** Pass 2: remove positions that are already in the graph ***/ + TRACKER_DBG(" redundancy removal..."); + + pos = consolidated_head_pos; + read_index = consolidated_head_index; + write_index = read_index + 1; + + // Keep track of the index/position from where we may overwrite previously copied deltas. + size_t overwrite_index = write_index; + ipos_t pos_at_overwrite_index = pos; + bool was_close = false; + + while (read_index < graph_next_write - 1 && !graph_fault) { + bool is_jump; + ipos_t prev_pos = pos; + ipos_t delta = walk_forward(read_index, is_jump); + pos += delta; + + // We copy each position optimistically, even if it turns out that it will be deleted later on. + size_t prev_write_index = write_index; + push_delta(write_index, delta, is_jump); + + // Scan the graph to see if any line is close to the current position + bool is_close = is_close_to_graph(pos, overwrite_index > GRAPH_SEARCH_RANGE ? overwrite_index - GRAPH_SEARCH_RANGE : 0, + overwrite_index - 1, pos_at_overwrite_index); + + // If the last position wasn't close to the graph, we must definitely keep this one. + if (!was_close) { + overwrite_index = write_index; + pos_at_overwrite_index = pos; + } + + // As soon as we encounter a position which is not close to the graph, we see if we can remove some preceeding positions + if (!is_close) { + + // If the last few positions were close to the graph, replace them by a jump + if (overwrite_index < prev_write_index) { + size_t max_space = prev_write_index - overwrite_index; + + if (push_delta(overwrite_index, prev_pos - pos_at_overwrite_index, true, max_space)) { + // We append the last position once again and go on from there + push_delta(write_index = overwrite_index, delta, is_jump); + + TRACKER_DBG(" replaced redundant path of size %zu by a jump at %zu", max_space, overwrite_index - 1); + } + } + + // If this branch was taken, the last important position will be re-initialized once it becomes relevant + } + + was_close = is_close; + } + + graph_next_write = write_index; + + + /*** Pass 3: detect nodes ***/ + TRACKER_DBG(" node detection..."); + + pos = consolidated_head_pos; + read_index = consolidated_head_index; + + while (read_index < graph_next_write - 1 && !graph_fault) { + + ipos_t inner_pos = pos; + size_t search_index = read_index; + bool inhibit_node = true; + + bool is_jump; + ipos_t delta = walk_forward(read_index, is_jump); + pos += delta; + + int granularity = get_granularity_at(pos); + size_t search_bound = read_index > GRAPH_SEARCH_RANGE ? read_index - GRAPH_SEARCH_RANGE : 0; + + while (search_index > search_bound && !graph_fault) { + + // Prepare a node in case we need it + node_t node = { + .index1 = (uint16_t)read_index, + .index2 = (uint16_t)search_index, + .dirty = 1, + .coef1 = 0, + .obsolete = 0, + .coef2 = 0, + .delta = pack_compact_delta_item({ .x = 0, .y = 0, .z = 0 }), + .use_line2 = 0, + .go_forward = 0, + .distance = MAX_DISTANCE + }; + + bool is_inner_jump; + int coef1, coef2; + //TRACKER_DBG(" index %zu is at %d %d %d", search_index, inner_pos.x, inner_pos.y, inner_pos.z); + ipos_t inner_delta = walk_backward(search_index, is_inner_jump); + + // We don't want to create a node to the line that directly precedes the current one + if (!inhibit_node) { + ipos_t line_to_line = get_line_to_line_delta(delta, pos, inner_delta, inner_pos, coef1, coef2, is_jump, is_inner_jump); + + if (dot(line_to_line) <= granularity) { + // If the node would be at the beginning of the line, we defer the node creation to the next line (unless we're about to terminate the search) + if (coef1 < MAX_COEFFICIENT || search_index <= search_bound) { + + if (coef1 < 0 || coef1 > MAX_COEFFICIENT || coef2 < 0 || coef2 > MAX_COEFFICIENT) { + GRAPH_ERR("delta coefficient out of range: have %d and %d", coef1, coef2); + return; + } + + node.coef1 = coef1; + node.coef2 = coef2; + node.delta = pack_compact_delta_item(line_to_line); + + //TRACKER_DBG(" line to line delta (%d %d %d)...(%d %d %d) to (%d %d %d)...(%d %d %d) is (%d %d %d), coef is %d, %d", + // delta.x, delta.y, delta.z, pos.x, pos.y, pos.z, + // inner_delta.x, inner_delta.y, inner_delta.z, inner_pos.x, inner_pos.y, inner_pos.z, + // line_to_line.x, line_to_line.y, line_to_line.z, + // coef1, coef2); + + TRACKER_DBG(" create node from %zu-%.2f to %zu-%.2f", (size_t)node.index1, (double)coef_to_float(node.coef1), + (size_t)node.index2, (double)coef_to_float(node.coef2)); + + push_node(node, granularity); + } + } + } + + inhibit_node = false; + inner_pos -= inner_delta; + } + } + + + // Finalize the consolidation + if (consolidated_head_index < pinned_index) { + graph_version++; + pinned_index = 0; + } + + consolidated_head_pos = graph_head_pos; + consolidated_head_index = graph_next_write - 1; + + + // Sanitize home position + size_t home_index; + unsigned int home_coef; + home_on_graph = get_closest_position(home_position, home_index, home_coef); + + if (nodes->index1 != home_index || nodes->coef1 != home_coef) { + nodes->index1 = home_index; + nodes->coef1 = home_coef; + have_dirty_nodes = true; + } + + TRACKER_DBG("consolidation complete, graph reduced to %zu", graph_next_write - 1); + DUMP_GRAPH(); +} + +float Tracker::measure_distance(size_t index1, int coef1, size_t index2, int coef2) +{ + // make sure position 1 is before position 2 + if (index1 > index2 || (index1 == index2 && coef1 < coef2)) { + myswap(index1, index2); + myswap(coef1, coef2); + } + + + bool is_jump; + ipos_t delta = fetch_delta(index1, is_jump); + float coef1_to_line_end = fast_sqrt(dot(apply_coef(delta, coef1)), false); + + // Walk along the interval to measure distance + float distance = 0; + + while (index1 < index2) { + delta = walk_forward(index1, is_jump); + distance += fast_sqrt(dot(delta), false); + + if (is_jump) { + return INFINITY; + } + } + + float coef2_to_line_end = fast_sqrt(dot(apply_coef(delta, coef2)), false); + return coef1_to_line_end + distance - coef2_to_line_end; +} + +bool Tracker::walk_to_node(size_t &index, int &coef, float &distance, bool forward, size_t search_bound) +{ + // Look through all nodes to find the closest one that lies in the direction of travel + size_t best_index = search_bound; + int best_coef = 0; + bool have_node = false; + + for (size_t i = 0; i < node_count; i++) { + if (forward) { + if ((node_at(i).index1 < best_index || (node_at(i).index1 == best_index && node_at(i).coef1 >= best_coef)) && + (node_at(i).index1 > index || (node_at(i).index1 == index && node_at(i).coef1 < coef))) { + best_index = node_at(i).index1; + best_coef = node_at(i).coef1; + have_node = true; + } + + if (i && (node_at(i).index2 < best_index || (node_at(i).index2 == best_index && node_at(i).coef2 >= best_coef)) && + (node_at(i).index2 > index || (node_at(i).index2 == index && node_at(i).coef2 < coef))) { + best_index = node_at(i).index2; + best_coef = node_at(i).coef2; + have_node = true; + } + + } else { + if ((node_at(i).index1 > best_index || (node_at(i).index1 == best_index && node_at(i).coef1 <= best_coef)) && + (node_at(i).index1 < index || (node_at(i).index1 == index && node_at(i).coef1 > coef))) { + best_index = node_at(i).index1; + best_coef = node_at(i).coef1; + have_node = true; + } + + if (i && (node_at(i).index2 > best_index || (node_at(i).index2 == best_index && node_at(i).coef2 <= best_coef)) && + (node_at(i).index2 < index || (node_at(i).index2 == index && node_at(i).coef2 > coef))) { + best_index = node_at(i).index2; + best_coef = node_at(i).coef2; + have_node = true; + } + } + } + + if (!have_node) { + return false; + } + + distance = measure_distance(index, coef, best_index, best_coef); + + index = best_index; + coef = best_coef; + + return !isinf(distance); +} + +float Tracker::apply_node_delta(size_t &index, unsigned int &coef, ipos_t *delta, bool &go_forward) +{ + float best_distance = INFINITY; + size_t best_index = index; + int best_coef = coef; + + if (delta) { + *delta = { .x = 0, .y = 0, .z = 0 }; + } + + // Consider all intersections at the specified position and use the one with the smallest distance home. + for (size_t i = 0; i < node_count; i++) { + if ((node_at(i).index1 == index && node_at(i).coef1 == coef) || (node_at(i).index2 == index + && node_at(i).coef2 == coef)) { + float distance = node_at(i).distance == MAX_DISTANCE ? FLT_MAX : (float)node_at(i).distance; + + // If the node is far from home, we arbitrarily prefer the line with the lowest index. + // This is in line with the decision that we prefer to walk backward. Thus, in case of + // unknown distances, at least we strictly minimize the index and don't get stuck in an infinite loop. + if (distance >= FLT_MAX) { + node_at(i).use_line2 = node_at(i).index2 < node_at(i).index1; + } + + if (best_distance > distance) { + best_distance = distance; + best_index = node_at(i).use_line2 ? node_at(i).index2 : node_at(i).index1; + best_coef = node_at(i).use_line2 ? node_at(i).coef2 : node_at(i).coef1; + go_forward = node_at(i).go_forward; + + if (delta) { + // If we don't change lines, the delta is 0 + if (index == (node_at(i).use_line2 ? node_at(i).index2 : node_at(i).index1)) { + *delta = { .x = 0, .y = 0, .z = 0 }; + + } else if (node_at(i).use_line2) { + *delta = unpack_compact_delta_item(node_at(i).delta); + + } else { + *delta = -unpack_compact_delta_item(node_at(i).delta); + } + } + } + } + } + + index = best_index; + coef = best_coef; + return best_distance; +} + +float Tracker::get_node_distance(size_t index, unsigned int coef, int &direction) +{ + size_t old_index = index; + unsigned int old_coef = coef; + bool go_forward = false; + float distance = apply_node_delta(index, coef, nullptr, go_forward); + direction = (index == old_index && coef == old_coef && !isinf(distance)) ? go_forward ? 1 : -1 : 0; + return distance; +} + +bool Tracker::set_node_distance(size_t index, int coef, float distance, bool go_forward) +{ + bool improvement = false; + + // Update all intersections at the specified position (if the new distance is an improvement) + for (size_t i = 0; i < node_count; i++) { + if ((node_at(i).index1 == index && node_at(i).coef1 == coef) || (node_at(i).index2 == index + && node_at(i).coef2 == coef)) { + float new_distance = distance + fast_sqrt(dot(unpack_compact_delta_item(node_at(i).delta)), false); + int int_distance = new_distance > MAX_DISTANCE ? MAX_DISTANCE : ceil(new_distance); + + if (node_at(i).distance > int_distance) { + TRACKER_DBG(" distance at %zu, improved from %.1f to %d (%.2f) (go %s from %zu-%.2f)", i, + node_at(i).distance >= MAX_DISTANCE ? (double)INFINITY : (double)node_at(i).distance, int_distance, (double)distance, + go_forward ? "forward" : "backward", index, (double)coef_to_float(coef)); + + node_at(i).distance = int_distance; + node_at(i).use_line2 = index == node_at(i).index2 && coef == node_at(i).coef2; + node_at(i).go_forward = go_forward; + node_at(i).dirty = true; + improvement = true; + have_dirty_nodes = true; + } + } + } + + return improvement; +} + +void Tracker::refresh_distances() +{ + if (!graph_next_write) { + return; + } + + // Only log if we're gonna do something + bool should_log = have_dirty_nodes; + + if (should_log) { + TRACKER_DBG("calculating distances..."); + } + + // This loop is guaranteed to terminate. + // Proof outline: + // The loop terminates when the set of dirty nodes is empty at the end of an iteration. This set is + // cleared at the beginning of every iteration. A node only gets added to this set, if its distance- + // to-home improves. A node distance cannot be increased and there are no intervals with negative + // length, so every node distance will eventually reach a lower bound. When this state is reached, + // the set of dirty nodes remains empty during an entire iteration, thus the loop terminates. + + while (have_dirty_nodes && !graph_fault) { + have_dirty_nodes = false; + + for (size_t i = 0; i < node_count; i++) { + // At every dirty node, we look in all four directions of the intersection. + if (node_at(i).dirty) { + node_at(i).dirty = 0; + + node_t node = node_at(i); + + size_t index; + int coef; + float interval_length; + + int direction_from_here, direction_from_there; + float distance_here = get_node_distance(node.index1, node.coef1, direction_from_here); + float distance_there; + + // Note that for instance if the current node has the instruction to walk forward, we certainly don't + // want it to be the destination of the next node in that direction (this could happen due to + // discretization of distances). That's why we respect the direction_from_here and direction_from_there. + + // Walk forward from index1 + if (walk_to_node(index = node.index1, coef = node.coef1, interval_length = 0, true, graph_next_write - 1)) { + distance_there = get_node_distance(index, coef, direction_from_there); + + if (direction_from_here != 1) { + set_node_distance(index, coef, distance_here + interval_length, false); + } + + if (direction_from_there != -1) { + set_node_distance(node.index1, node.coef1, distance_there + interval_length, true); + } + } + + // Walk backward from index1 + if (walk_to_node(index = node.index1, coef = node.coef1, interval_length = 0, false, 0)) { + distance_there = get_node_distance(index, coef, direction_from_there); + + if (direction_from_here != -1) { + set_node_distance(index, coef, distance_here + interval_length, true); + } + + if (direction_from_there != 1) { + set_node_distance(node.index1, node.coef1, distance_there + interval_length, false); + } + } + + // Ignore index2 of home node + if (!i) { + continue; + } + + distance_here = get_node_distance(node.index2, node.coef2, direction_from_here); + + // Walk forward from index2 + if (walk_to_node(index = node.index2, coef = node.coef2, interval_length = 0, true, graph_next_write - 1)) { + distance_there = get_node_distance(index, coef, direction_from_there); + + if (direction_from_here != 1) { + set_node_distance(index, coef, distance_here + interval_length, false); + } + + if (direction_from_there != -1) { + set_node_distance(node.index2, node.coef2, distance_there + interval_length, true); + } + } + + // Walk backward from index2 + if (walk_to_node(index = node.index2, coef = node.coef2, interval_length = 0, false, 0)) { + distance_there = get_node_distance(index, coef, direction_from_there); + + if (direction_from_here != -1) { + set_node_distance(index, coef, distance_here + interval_length, true); + } + + if (direction_from_there != 1) { + set_node_distance(node.index2, node.coef2, distance_there + interval_length, false); + } + } + } + } + } + +#ifdef DEBUG_TRACKER + + if (should_log) { + dump_nodes(); + } + +#endif +} + +bool Tracker::calc_return_path(path_finding_context_t &context, bool &progress) +{ + // Before making any decision, make sure we know the distance of every node to home. + refresh_distances(); + + TRACKER_DBG("calculate return path from %zu-%.2f (%d %d %d) to (%d %d %d)", context.current_index, + (double)coef_to_float(context.current_coef), + context.current_pos.x, context.current_pos.y, context.current_pos.z, + home_on_graph.x, home_on_graph.y, home_on_graph.z); + + progress = false; + + ipos_t delta = { .x = 0, .y = 0, .z = 0 }; + + // If we reached the checkpoint, re-evaluate next checkpoint and potentially switch lines. + if (context.current_index == context.checkpoint_index && context.current_coef == context.checkpoint_coef) { + TRACKER_DBG("checkpoint!"); + + // This check is no longer neccessary + if (context.checkpoint_index == nodes->index1 && context.checkpoint_coef == nodes->coef1) { + return false; + } + + // If we're at a node (which doesn't have to be the case), this will switch to the line that gives the best path home. + bool go_forward = false; + float dist_through_node = apply_node_delta(context.current_index, context.current_coef, &delta, go_forward); + + // If we're at a node, we must proceed in the recommended direction, otherwise we risk going in the + // wrong direction (in the case of equal cost, due to rounding) and ending up in an infinite loop. + // Also, we don't want to arrive at a node exactly from the direction where it recommends to go. + + int direction = 0; + + // Determine cost and bound of walking forward (except if the current node recommended to go backward) + float forward_dist; + size_t forward_node_index = context.current_index; + int forward_node_coef = context.current_coef; + + if ((isinf(dist_through_node) || go_forward) + && walk_to_node(forward_node_index, forward_node_coef, forward_dist = 0, true, graph_next_write - 1)) { + forward_dist += get_node_distance(forward_node_index, forward_node_coef, direction); + + } else { + forward_dist = INFINITY; + } + + if (direction == -1) { + forward_dist = INFINITY; + } + + // Determine cost and bound of walking backward (except if the current node recommended to go forward) + float backward_dist; + size_t backward_node_index = context.current_index; + int backward_node_coef = context.current_coef; + + if ((isinf(dist_through_node) || !go_forward) + && walk_to_node(backward_node_index, backward_node_coef, backward_dist = 0, false, 0)) { + backward_dist += get_node_distance(backward_node_index, backward_node_coef, direction); + + } else { + backward_dist = INFINITY; + } + + if (direction == 1) { + backward_dist = INFINITY; + } + + TRACKER_DBG("distance from %zu-%.2f is %f forward (to %zu-%.2f) and %f backward (to %zu-%.2f)", context.current_index, + (double)coef_to_float(context.current_coef), (double)forward_dist, forward_node_index, + (double)coef_to_float(forward_node_coef), (double)backward_dist, backward_node_index, + (double)coef_to_float(backward_node_coef)); + + // Abort if we don't know what to do + if (isinf(backward_dist) && forward_dist >= FLT_MAX) { + PX4_WARN("Could not find any path home from %zu-%.2f.", context.current_index, + (double)coef_to_float(context.current_coef)); + return false; + } + + // Note that we prefer to walk backward + if (forward_dist < backward_dist) { + context.checkpoint_index = forward_node_index; + context.checkpoint_coef = forward_node_coef; + + } else { + context.checkpoint_index = backward_node_index; + context.checkpoint_coef = backward_node_coef; + } + + pin_index(context.current_index); + pin_index(context.checkpoint_index); + } + + // If the (potential) node switch above did not yield a position update yet, advance in the direction of the next checkpoint + if (!(delta.x || delta.y || delta.z)) { + bool is_jump; + ipos_t current_delta = fetch_delta(context.current_index, is_jump); + delta = apply_coef(current_delta, context.current_coef); + + if (context.current_index < context.checkpoint_index) { + current_delta = walk_forward(context.current_index, is_jump); + + if (is_jump) { + PX4_WARN("path finding algorithm is moving forward along jump delta"); + } + + context.current_coef = MAX_COEFFICIENT + 1; + + } else if (context.current_index > context.checkpoint_index) { + delta -= current_delta; + + if (is_jump) { + PX4_WARN("path finding algorithm is moving backward along jump delta"); + } + + current_delta = walk_backward(context.current_index, is_jump); + context.current_coef = 0; + + } else { + if (is_jump) { + PX4_WARN("path finding algorithm is moving along jump delta"); + } + + delta -= apply_coef(current_delta, context.checkpoint_coef); + context.current_coef = context.checkpoint_coef; + } + + pin_index(context.current_index); + + //TRACKER_DBG("changing position by (%d %d %d)", delta.x, delta.y, delta.z); + } + + context.current_pos += delta; + progress = delta.x || delta.y || delta.z; + + return true; +} + +void Tracker::mark_obsolete(size_t index, int coef, bool forward, bool backward, size_t retain, size_t lower_bound, + size_t upper_bound) +{ + size_t start_index = index; + int start_coef = coef; + + TRACKER_DBG(" mark as obsolete from %zu-%.2f %s", index, (double)coef_to_float(coef), + forward ? backward ? "both dirs" : "forward" : backward ? "backward" : "nothing"); + + bool is_jump; + + size_t prev_index = index - (((graph[index] >> 15) & 1) ? FAR_DELTA_SIZE : 1); + + while (index < upper_bound && forward && !graph_fault) { + forward = graph[index] != OBSOLETE_DELTA && index != retain; + bool keep = index == start_index || !forward; + + // Keep delta if any nodes lie on it. + // If one is even in the right direction, finish walk. + for (size_t i = 0; i < node_count && forward; i++) { + if (!node_at(i).obsolete) { + keep |= node_at(i).index1 == index || (i && node_at(i).index2 == index); + + if ((node_at(i).index1 == index && node_at(i).coef1 <= coef) + || (i && node_at(i).index2 == index && node_at(i).coef2 <= coef)) { + forward = false; + } + } + } + + while (!keep && prev_index++ < index) { + graph[prev_index] = OBSOLETE_DELTA; + } + + prev_index = index; + walk_forward(index, is_jump); + coef = MAX_COEFFICIENT + 1; + } + + + index = start_index; + coef = start_coef; + + while (index > lower_bound && backward && !graph_fault) { + backward = graph[index] != OBSOLETE_DELTA && index != retain; + bool keep = index == start_index || !backward; + + // See if any node lies on this delta. + // If there are nodes, but they don't lie in the considered direction, we just keep this delta but continue the walk. + for (size_t i = 0; i < node_count && backward; i++) { + if (!node_at(i).obsolete) { + keep |= node_at(i).index1 == index || (i && node_at(i).index2 == index); + + if ((node_at(i).index1 == index && node_at(i).coef1 >= coef) + || (i && node_at(i).index2 == index && node_at(i).coef2 >= coef)) { + backward = false; + } + } + } + + prev_index = index; + walk_backward(index, is_jump); + coef = 0; + + while (!keep && prev_index-- > index) { + graph[prev_index + 1] = OBSOLETE_DELTA; + } + } +} + +void Tracker::rewrite_graph() +{ + if (!graph_next_write) { + return; + } + + consolidate_graph("graph rewrite requested"); + + TRACKER_DBG("rewriting graph..."); + + // The cache helps us in case there is no space at all for the rewrite area + delta_item_t rewrite_cache[REWRITE_CACHE_SIZE]; + size_t cache_size = 0; + + // Init rewrite area at the end of the node buffer + size_t next_rewrite_index = GRAPH_LENGTH - node_count * sizeof(node_t) / sizeof(delta_item_t) - 1; + + // Init path finding context, using the graph head as starting point + path_finding_context_t context = { + .current_pos = graph_head_pos, + .current_index = graph_next_write - 1, + .current_coef = 0, + .checkpoint_index = 0, + .checkpoint_coef = 0, + .graph_version = graph_version + }; + context.checkpoint_index = context.current_index; + context.checkpoint_coef = context.current_coef; + + bool not_home = true; + + // Walk the shortest path home and reconstruct it at the end of the graph buffer. + while (not_home && !graph_fault) { + + size_t prev_checkpoint_index = context.checkpoint_index; + int prev_checkpoint_coef = context.checkpoint_coef; + bool at_checkpoint = context.current_index == context.checkpoint_index + && context.current_coef == context.checkpoint_coef; + + // Proceed on the return path + bool progress = false; + ipos_t prev_pos = context.current_pos; + not_home = calc_return_path(context, progress); + + // Mark as obsolete the direction in which we're not going + bool going_forward = context.current_index < context.checkpoint_index + || (context.current_index == context.checkpoint_index && context.current_coef > context.checkpoint_coef); + bool going_backward = context.current_index > context.checkpoint_index + || (context.current_index == context.checkpoint_index && context.current_coef < context.checkpoint_coef); + mark_obsolete(context.current_index, context.current_coef, going_backward, going_forward, 0, 0, graph_next_write); + + // At every checkpoint (usually a node), mark all irrelevant intervals as obsolete + if (at_checkpoint) { + for (size_t i = 1; i < node_count; i++) { + bool come_from_line1 = node_at(i).index1 == prev_checkpoint_index && node_at(i).coef1 == prev_checkpoint_coef; + bool come_from_line2 = node_at(i).index2 == prev_checkpoint_index && node_at(i).coef2 == prev_checkpoint_coef; + + if (come_from_line1 || come_from_line2) { + bool use_line2 = node_at(i).use_line2; + node_at(i).obsolete = 1; + + TRACKER_DBG(" node %zu is obsolete and I'm going %s on line %d", i, + going_forward ? "forward" : going_backward ? "backward" : "nowhere", use_line2 ? 2 : 1); + + // Remove intervals from line1 + mark_obsolete(node_at(i).index1, node_at(i).coef1, + use_line2 || going_backward, + use_line2 || going_forward, + node_at(i).index2, 0, graph_next_write); + + // Remove intervals from line2 + mark_obsolete(node_at(i).index2, node_at(i).coef2, + !use_line2 || going_backward, + !use_line2 || going_forward, + node_at(i).index1, 0, graph_next_write); + } + } + } + + bool cache_full = false; + + // Push the negative delta to the cache (when flushing, the deltas will be reversed) + TRACKER_DBG(" advance from %d %d %d to %d %d %d", prev_pos.x, prev_pos.y, prev_pos.z, context.current_pos.x, + context.current_pos.y, context.current_pos.z); + + if (progress) { + cache_full = !push_delta(cache_size, prev_pos - context.current_pos, false, REWRITE_CACHE_SIZE - cache_size, + rewrite_cache); + } + + // Once in a while, flush the cache to the rewrite area + // todo: recall the reasoning here: why would multiple cache flushes make sense? + while ((!not_home || cache_full) && cache_size && !graph_fault) { + TRACKER_DBG(" flush rewrite cache of size %zu", cache_size); + + // If the cache cannot be flushed, reclaim space used by obsolete stuff + if (next_rewrite_index < graph_next_write - 1 + cache_size) { + TRACKER_DBG(" reclaim space"); + + // Remove obsolete nodes + size_t old_node_count = node_count; + remove_nodes(0, 0); + + // Move rewrite area to the end of the available space + size_t yield = (old_node_count - node_count) * sizeof(node_t) / sizeof(delta_item_t); + size_t rewrite_length = GRAPH_LENGTH - old_node_count * sizeof(node_t) / sizeof(delta_item_t) - next_rewrite_index - 1; + memmove(graph + next_rewrite_index + 1 + yield, graph + next_rewrite_index + 1, rewrite_length * sizeof(delta_item_t)); + next_rewrite_index += yield; + + // Remove obsolete delta elements + size_t copy_destination = 0; + size_t copy_origin = 0; + size_t copy_length = 0; + + for (size_t i = 0; i <= graph_next_write; i++) { + if (i == graph_next_write ? true : (graph[i] == OBSOLETE_DELTA)) { + if (copy_length) { + // Update indices that point to shifted deltas + context.current_index -= context.current_index < copy_origin ? 0 : copy_origin - copy_destination; + context.checkpoint_index -= context.checkpoint_index < copy_origin ? 0 : copy_origin - copy_destination; + + for (size_t j = 0; j < node_count; j++) { + node_at(j).index1 -= node_at(j).index1 < copy_origin ? 0 : copy_origin - copy_destination; + node_at(j).index2 -= node_at(j).index2 < copy_origin ? 0 : copy_origin - copy_destination; + } + + memmove(graph + copy_destination, graph + copy_origin, copy_length * sizeof(delta_item_t)); + copy_destination += copy_length; + copy_length = 0; + } + + copy_origin = i + 1; + + } else { + copy_length++; + } + } + + TRACKER_DBG(" reclaimed memory of %zu elements", yield + graph_next_write - copy_destination); + graph_next_write = copy_destination; + } + + + // Flush rewrite cache to rewrite area (as much as possible) + size_t copy_count = math::min(cache_size, next_rewrite_index - graph_next_write + 1); + + for (size_t i = 0; i < copy_count; i++) { + if (!next_rewrite_index) { + GRAPH_ERR("rewrite area exceeded graph buffer size"); + return; + } + + if ((rewrite_cache[i] >> 15) & 1) { + graph[next_rewrite_index--] = rewrite_cache[i + 2]; + graph[next_rewrite_index--] = rewrite_cache[i + 1]; + graph[next_rewrite_index--] = rewrite_cache[i]; + i += 2; + + } else { + graph[next_rewrite_index--] = rewrite_cache[i]; + } + } + + memmove(rewrite_cache, rewrite_cache + copy_count, (cache_size -= copy_count) * sizeof(delta_item_t)); + } + + // If the delta push failed previously, it should work now that we have flushed the cache + if (cache_full) { + if (!push_delta(cache_size, prev_pos - context.current_pos, false, REWRITE_CACHE_SIZE - cache_size, rewrite_cache)) { + GRAPH_ERR("no space in graph rewrite cache after flushing"); + return; + } + + not_home = true; // just in case, pretend we're not yet home, so we get get another chance to flush the cache + } + } + + // Move rebuilt path to the beginning of the buffer. + size_t rewrite_length = GRAPH_LENGTH - node_count * sizeof(node_t) / sizeof(delta_item_t) - next_rewrite_index - 1; + memmove(graph + 1, graph + next_rewrite_index + 1, rewrite_length * sizeof(delta_item_t)); + + // Finalize rewrite + node_count = 1; + graph_next_write = rewrite_length + 1; + consolidated_head_index = 0; + graph_version++; + pinned_index = 0; + + DUMP_GRAPH(); + TRACKER_DBG("graph rewrite complete!"); +} + +void Tracker::reset_graph() +{ + TRACKER_DBG("deleting graph of length %zu", graph_next_write); + node_count = 1; + graph_next_write = 0; + consolidated_head_index = 0; + graph_version++; + pinned_index = 0; + memory_pressure = 1; + graph_fault = false; +} + +bool Tracker::init_return_path(path_finding_context_t &context, float &x, float &y, float &z) +{ + // We prefer that the index we're about to expose remains valid, so we consolidate the graph first + consolidate_graph("init return path"); + + if (!graph_next_write) { + return false; + } + + // Use graph head as starting point + context = { + .current_pos = graph_head_pos, + .current_index = graph_next_write - 1, + .current_coef = 0, + .checkpoint_index = 0, + .checkpoint_coef = 0, + .graph_version = graph_version + }; + + context.checkpoint_index = context.current_index; + context.checkpoint_coef = context.current_coef; + + pin_index(context.current_index); + + x = context.current_pos.x; + y = context.current_pos.y; + z = context.current_pos.z; + + return true; +} + +bool Tracker::init_return_path_global(path_finding_context_t &context, double &lat, double &lon, float &alt) +{ + float x, y, z; + bool result = init_return_path(context, x, y, z); + map_projection_reproject(&_ref_pos, x, y, &lat, &lon); + alt = _ref_alt - z; + return result; +} + +bool Tracker::advance_return_path(path_finding_context_t &context, float &x, float &y, float &z) +{ + // If the graph version changed, we need to sanitize the path finding context + if (context.graph_version != graph_version) { + consolidate_graph("advance return path with invalid index"); // We need to make sure that consolidation is complete before continuing + context.current_pos = get_closest_position(context.current_pos, context.current_index, context.current_coef); + context.checkpoint_index = context.current_index; + context.checkpoint_coef = context.current_coef; + pin_index(context.current_index); + } + + bool progress = false; + bool result = calc_return_path(context, progress); + + // We give a second chance to make progress: + // It may happen for example that we switch from the end of one line to the beginning of the next line, but the position stays unchanged. + if (result && !progress) { + TRACKER_DBG("return path made no progress - second chance"); + result = calc_return_path(context, progress); + } + + x = context.current_pos.x; + y = context.current_pos.y; + z = context.current_pos.z; + + return result; +} + +bool Tracker::advance_return_path_global(path_finding_context_t &context, double &lat, double &lon, float &alt) +{ + float x, y, z; + bool result = advance_return_path(context, x, y, z); + map_projection_reproject(&_ref_pos, x, y, &lat, &lon); + alt = _ref_alt - z; + return result; +} + +bool Tracker::is_context_close_to_head(path_finding_context_t &context) +{ + ipos_t delta = context.current_pos - to_ipos(last_known_position); + /*TRACKER_DBG("context (%d %d %d) to head (%d %d %d) is sqrt(%d)", + context.current_pos.x, context.current_pos.y, context.current_pos.z, + graph_head_pos.x, graph_head_pos.y, graph_head_pos.z, + dot(delta));*/ + return dot(delta) <= get_granularity_at(context.current_pos); +} + +bool Tracker::is_context_close_to_home(path_finding_context_t &context) +{ + ipos_t delta = context.current_pos - home_on_graph; + return dot(delta) <= get_granularity_at(context.current_pos); +} + +bool Tracker::is_same_pos(path_finding_context_t &context1, path_finding_context_t &context2) +{ + return context1.current_index == context2.current_index && context1.current_coef == context2.current_coef; +} + +void Tracker::dump_recent_path() +{ + if (recent_path_next_read == RECENT_PATH_LENGTH) { + PX4_INFO("recent path empty"); + return; + } + + PX4_INFO("recent path:"); + size_t recent_path_length = (recent_path_next_write + RECENT_PATH_LENGTH - recent_path_next_read - 1) % + RECENT_PATH_LENGTH + 1; + PX4_INFO(" length: %zu (read-ptr: %zu, write-ptr: %zu)", recent_path_length, recent_path_next_read, + recent_path_next_write); + + size_t index = recent_path_next_write; + ipos_t head = recent_path_head; + + do { + PX4_INFO(" (%d, %d, %d)", head.x, head.y, head.z); + + index = (index ? index : RECENT_PATH_LENGTH) - 1; + + if ((recent_path[index] >> 15) & 1) { + PX4_INFO("ignore the last position"); + } + + head -= unpack_compact_delta_item(recent_path[index]); + } while (index != recent_path_next_read); +} + +void Tracker::dump_graph() +{ + if (!graph_next_write) { + PX4_INFO("full path empty"); + return; + } + + PX4_INFO("full path (%zu elements of size %zu bytes, %.2f%% full, pressure %d, tracker overhead is %zu bytes):", + graph_next_write, (sizeof(delta_item_t) * CHAR_BIT) / 8, + (double)100.0 * ((double)1 - (double)get_free_graph_space() / (GRAPH_LENGTH * sizeof(delta_item_t))), + memory_pressure, + sizeof(Tracker) - sizeof(graph)); + PX4_INFO(" home: %u-%.2f (%d %d %d)", nodes->index1, (double)coef_to_float(nodes->coef1), + home_on_graph.x, home_on_graph.y, home_on_graph.z); + + ipos_t pos = graph_head_pos; + + size_t prev_index = graph_next_write; + size_t index = graph_next_write - 1; + + while (prev_index) { + prev_index = index; + bool is_jump; + ipos_t delta = walk_backward(index, is_jump); + + PX4_INFO(" %zu: (%d, %d, %d)%s%s", + prev_index, pos.x, pos.y, pos.z, + is_jump ? ", jump" : "", + nodes->index1 == prev_index ? " <= home" : ""); + + pos -= delta; + }; + + dump_nodes(); +} + +void Tracker::dump_nodes() +{ + PX4_INFO("nodes (%zu elements of size %zu bytes)%s:", node_count, (sizeof(node_t) * CHAR_BIT) / 8, + have_dirty_nodes ? " *" : ""); + + for (size_t i = 0; i < node_count; i++) { + PX4_INFO(" node %zu: %s%zu-%.2f%s to %s%zu-%.2f%s, expansion %.3f, dist to home %d %s%s%s", i, + node_at(i).use_line2 ? " " : ">", (size_t)node_at(i).index1, (double)coef_to_float(node_at(i).coef1), + node_at(i).use_line2 ? " " : "<", + node_at(i).use_line2 ? ">" : " ", (size_t)node_at(i).index2, (double)coef_to_float(node_at(i).coef2), + node_at(i).use_line2 ? "<" : " ", + (double)fast_sqrt(dot(unpack_compact_delta_item(node_at(i).delta)), false), node_at(i).distance, + node_at(i).go_forward ? "forward" : "backward", + node_at(i).dirty ? " *" : "", + i ? "" : " <= home"); + } +} + +void Tracker::dump_path_to_home() +{ + path_finding_context_t context; + + float x, y, z; + + if (!init_return_path(context, x, y, z)) { + PX4_INFO("shortest path to home unavailable"); + return; + } + + PX4_INFO("shortest path to home:"); + + do { + PX4_INFO(" %zu-%.2f: (%d, %d, %d)", context.current_index, (double)coef_to_float(context.current_coef), (int)x, (int)y, + (int)z); + } while (advance_return_path(context, x, y, z)); +} diff --git a/src/modules/navigator/tracker.h b/src/modules/navigator/tracker.h new file mode 100644 index 0000000000..cbe7aa9b15 --- /dev/null +++ b/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 + */ + +#pragma once + +#include +#include +#include +#include +#include + +// 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; +}; diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index fbfa260eb8..e8ac4415c0 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/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 diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 8b3d49756c..862fd70616 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/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[]);