From d43e4d88c34b6259da4b10232f1cdd59b87d50c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 May 2020 11:36:30 +0200 Subject: [PATCH] mavsdk_tests: wait until home is set properly Otherwise we might have NaN stored as home. --- test/mavsdk_tests/autopilot_tester.cpp | 13 ++++++------- test/mavsdk_tests/autopilot_tester.h | 3 +-- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 865562d4e8..7aac4e8d8c 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -78,7 +78,12 @@ void AutopilotTester::wait_until_ready_local_position_only() void AutopilotTester::store_home() { request_ground_truth(); - _home = get_ground_truth_position(); + std::cout << "Waiting to get home position" << std::endl; + CHECK(poll_condition_with_timeout( + [this]() { + _home = _telemetry->ground_truth(); + return std::isfinite(_home.latitude_deg) && std::isfinite(_home.longitude_deg); + }, std::chrono::seconds(10))); } void AutopilotTester::check_home_within(float acceptance_radius_m) @@ -241,12 +246,6 @@ bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::Pos void AutopilotTester::request_ground_truth() { CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); -} - -Telemetry::GroundTruth AutopilotTester::get_ground_truth_position() -{ - return _telemetry->ground_truth(); } bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 1d993b7715..cd3fbe8e50 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -86,10 +86,9 @@ private: const mavsdk::geometry::CoordinateTransformation::LocalCoordinate &local_coordinate, const MissionOptions &mission_options, const mavsdk::geometry::CoordinateTransformation &ct); - Telemetry::GroundTruth get_ground_truth_position(); bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m); - bool estimated_position_close_to(const Offboard::PositionNEDYaw &target_position, float acceptance_radius_m); + bool estimated_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m); bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m); mavsdk::Mavsdk _mavsdk{};