Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

mavsdk_tests: more debug info

Browse Source
This commit is contained in:
Julian Oes
2020-05-26 10:38:14 +02:00
parent 1a25cdf7b2
commit 160aca9207
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/autopilot_tester.cpp
+5 -1
View File
@@ -262,6 +262,10 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
CHECK(std::isfinite(current_pos.latitude_deg)); CHECK(std::isfinite(current_pos.latitude_deg));
CHECK(std::isfinite(current_pos.longitude_deg)); CHECK(std::isfinite(current_pos.longitude_deg));
LocalCoordinate local_pos = ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg}); LocalCoordinate local_pos = ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg});
const double distance = sqrt(sq(local_pos.north_m) + sq(local_pos.east_m));
return sq(local_pos.north_m) + sq(local_pos.east_m) < sq(acceptance_radius_m); std::cout << "Target: " << target_pos << std::endl;
std::cout << "Current: " << current_pos << std::endl;
std::cout << "Distance: " << distance << std::endl;
return distance < acceptance_radius_m;
} }