Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_SafeRTL: update example sketch 

remove pruning test because this is always performed after simplification
modify expected results to match latest algorithm
This commit is contained in:
Randy Mackay 2017-09-08 19:24:58 -07:00
parent 939ca36c33
commit 4415a6b052
2 changed files with 7 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -69,16 +69,6 @@ void loop()
run_time = AP_HAL::micros() - reference_time; run_time = AP_HAL::micros() - reference_time;
check_path(test_path_after_simplifying, "simplify", run_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 // test both simplification and pruning
hal.scheduler->delay(5); // delay 5 milliseconds because request_through_cleanup uses millisecond timestamps hal.scheduler->delay(5); // delay 5 milliseconds because request_through_cleanup uses millisecond timestamps
reset(); reset();
@ -87,7 +77,7 @@ void loop()
safe_rtl.run_background_cleanup(); safe_rtl.run_background_cleanup();
} }
run_time = AP_HAL::micros() - reference_time; 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 // delay before next display
hal.scheduler->delay(5e3); // 5 seconds hal.scheduler->delay(5e3); // 5 seconds

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

@ -294,73 +294,6 @@ std::vector<Vector3f> test_path_after_simplifying {
{300.0, 300.0, 295.0}, {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 { std::vector<Vector3f> test_path_complete {
{0.0, 0.0, 0.0}, // 0 {0.0, 0.0, 0.0}, // 0
{10.0, 0.0, 0.0}, {10.0, 0.0, 0.0},
@ -410,9 +343,10 @@ std::vector<Vector3f> test_path_complete {
{75.0, 55.0, 10.0}, {75.0, 55.0, 10.0},
{100.0, 100.0, 100.0}, {100.0, 100.0, 100.0},
{103.0, 100.0, 100.0}, {103.0, 100.0, 100.0},
{103.0, 100.0, 100.0},
{200.0, 200.0, 200.0}, {200.0, 200.0, 200.0},
{203.0, 200.0, 200.0}, {203.0, 200.0, 200.0}, // 50
{203.0, 203.0, 200.0}, // 50 {203.0, 203.0, 200.0},
{206.0, 203.0, 200.0}, {206.0, 203.0, 200.0},
{206.0, 206.0, 200.0}, {206.0, 206.0, 200.0},
{209.0, 206.0, 200.0}, {209.0, 206.0, 200.0},
@ -420,8 +354,9 @@ std::vector<Vector3f> test_path_complete {
{212.0, 209.0, 200.0}, {212.0, 209.0, 200.0},
{212.0, 212.0, 200.0}, {212.0, 212.0, 200.0},
{220.0, 220.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}, {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}, {300.0, 300.0, 295.0},
}; };