dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavsdk_tests: more debug info

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
test/mavsdk_tests/autopilot_tester.cpp
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.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;
}