zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_SafeRTL: update example sketch 

remove pruning test because this is always performed after simplification
modify expected results to match latest algorithm
mission-4.1.18
Randy Mackay 8 years ago
parent
939ca36c33
commit
4415a6b052
2 changed files with 7 additions and 82 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.cpp
  2. 77
      libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.h

12
libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.cpp
Unescape View File

@ -69,16 +69,6 @@ void loop() @@ -69,16 +69,6 @@ void loop()
run_time = AP_HAL::micros() - reference_time;
check_path(test_path_after_simplifying, "simplify", run_time);
// test pruning
hal.scheduler->delay(5); // delay 5 milliseconds because request_through_cleanup uses millisecond timestamps
reset();
reference_time = AP_HAL::micros();
while (!safe_rtl.request_thorough_cleanup(AP_SafeRTL::THOROUGH_CLEAN_PRUNE_ONLY)) {
safe_rtl.run_background_cleanup();
}
run_time = AP_HAL::micros() - reference_time;
check_path(test_path_after_pruning, "prune", run_time);
// test both simplification and pruning
hal.scheduler->delay(5); // delay 5 milliseconds because request_through_cleanup uses millisecond timestamps
reset();
@ -87,7 +77,7 @@ void loop() @@ -87,7 +77,7 @@ void loop()
safe_rtl.run_background_cleanup();
}
run_time = AP_HAL::micros() - reference_time;
check_path(test_path_complete, "both", run_time);
check_path(test_path_complete, "simplify and pruning", run_time);
// delay before next display
hal.scheduler->delay(5e3); // 5 seconds

77
libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.h
Unescape View File

@ -294,73 +294,6 @@ std::vector<Vector3f> test_path_after_simplifying { @@ -294,73 +294,6 @@ std::vector<Vector3f> test_path_after_simplifying {
{300.0, 300.0, 295.0},
};
std::vector<Vector3f> test_path_after_pruning {
{0.0, 0.0, 0.0}, // 0
{3.0, 0.0, 0.0},
{6.0, 0.0, 0.0},
{10.0, 0.0, 0.0},
{10.0, 3.0, 0.0},
{13.0, 3.0, 0.0},
{13.0, 6.0, 0.0},
{16.0, 6.0, 0.0},
{16.0, 8.0, 1.0},
{18.0, 8.0, 0.0},
{20.0, 10.0, 0.0}, // 10
{20.0, 10.0, 10.0},
{23.0, 10.0, 10.0},
{23.0, 13.0, 10.0},
{26.0, 13.0, 10.0},
{26.0, 16.0, 10.0},
{29.0, 16.0, 10.0},
{29.0, 19.0, 10.0},
{32.0, 19.0, 10.0},
{32.0, 22.0, 10.0},
{35.0, 22.0, 10.0}, // 20
{35.0, 25.0, 10.0},
{38.0, 25.0, 10.0},
{38.0, 28.0, 10.0},
{41.0, 28.0, 10.0},
{41.0, 31.0, 10.0},
{44.0, 31.0, 10.0},
{44.0, 34.0, 10.0},
{47.0, 34.0, 10.0},
{47.0, 37.0, 10.0},
{51.0, 37.0, 10.0}, // 30
{51.0, 41.0, 10.0},
{54.0, 41.0, 10.0},
{54.0, 44.0, 10.0},
{57.0, 44.0, 10.0},
{57.0, 47.0, 10.0},
{60.0, 47.0, 10.0},
{60.0, 40.0, 10.0},
{63.0, 40.0, 10.0},
{63.0, 43.0, 10.0},
{66.0, 43.0, 10.0}, // 40
{66.0, 46.0, 10.0},
{69.0, 46.0, 10.0},
{69.0, 49.0, 10.0},
{72.0, 49.0, 10.0},
{72.0, 52.0, 10.0},
{75.0, 52.0, 10.0},
{75.0, 55.0, 10.0},
{100.0, 100.0, 100.0},
{103.0, 100.0, 100.0},
{200.0, 200.0, 200.0}, // 50
{203.0, 200.0, 200.0},
{203.0, 203.0, 200.0},
{206.0, 203.0, 200.0},
{206.0, 206.0, 200.0},
{209.0, 206.0, 200.0},
{209.0, 209.0, 200.0},
{212.0, 209.0, 200.0},
{212.0, 212.0, 200.0},
{220.0, 220.0, 200.0},
{223.91304, 220.3913, 199.95653}, // 60
{229.0, 220.0, 200.0},
{300.2381, 300.0, 300.0},
{300.0, 300.0, 295.0},
};
std::vector<Vector3f> test_path_complete {
{0.0, 0.0, 0.0}, // 0
{10.0, 0.0, 0.0},
@ -410,9 +343,10 @@ std::vector<Vector3f> test_path_complete { @@ -410,9 +343,10 @@ std::vector<Vector3f> test_path_complete {
{75.0, 55.0, 10.0},
{100.0, 100.0, 100.0},
{103.0, 100.0, 100.0},
{103.0, 100.0, 100.0},
{200.0, 200.0, 200.0},
{203.0, 200.0, 200.0},
{203.0, 203.0, 200.0}, // 50
{203.0, 200.0, 200.0}, // 50
{203.0, 203.0, 200.0},
{206.0, 203.0, 200.0},
{206.0, 206.0, 200.0},
{209.0, 206.0, 200.0},
@ -420,8 +354,9 @@ std::vector<Vector3f> test_path_complete { @@ -420,8 +354,9 @@ std::vector<Vector3f> test_path_complete {
{212.0, 209.0, 200.0},
{212.0, 212.0, 200.0},
{220.0, 220.0, 200.0},
{223.91304, 220.3913, 199.95653},
{223.0, 220.0, 200.0},
{223.2368474, 220.0789542, 199.5263052}, // 60
{229.0, 220.0, 200.0},
{300.2381, 300.0, 300.0}, // 60
{300.1223662, 300.0, 300.0696305},
{300.0, 300.0, 295.0},
};

Write Preview
Loading…
Cancel
Save