From 160aca92072ef64e3f843f8555ef712dfd7d09b3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 May 2020 10:38:14 +0200 Subject: [PATCH] mavsdk_tests: more debug info --- test/mavsdk_tests/autopilot_tester.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 42d4eb5458..f02072b06d 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -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; }