From 176d932f23ac9e8909bfe5711cd65dae2d20119e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 10 Feb 2021 13:33:28 +0100 Subject: [PATCH] mavsdk_tests: prevent missing updates It seems like we are often missing updates from topics like flight mode or in air state, both topics that are sent out infrequenctly e.g. at 1 Hz. Therefore, instead of polling for that data we should probably subscribe to the updates and that way get notified of each an every update. For instance this should prevent the case where we miss the mode change from mission to descend and back to mission once landed and disarmed. --- test/mavsdk_tests/autopilot_tester.cpp | 154 ++++++++++--------------- test/mavsdk_tests/autopilot_tester.h | 3 + 2 files changed, 64 insertions(+), 93 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 57636515d8..599a1a879f 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -164,8 +164,7 @@ void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration) void AutopilotTester::wait_until_hovering() { - REQUIRE(poll_condition_with_timeout( - [this]() { return _telemetry->landed_state() == Telemetry::LandedState::InAir; }, std::chrono::seconds(30))); + wait_for_landed_state(Telemetry::LandedState::InAir, std::chrono::seconds(30)); } void AutopilotTester::prepare_square_mission(MissionOptions mission_options) @@ -240,55 +239,21 @@ void AutopilotTester::execute_mission_and_lose_gps() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { - std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + start_and_wait_for_first_mission_item(); - if (progress.current == 1) { - std::thread([this]() { - CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) - == Failure::Result::Success); - }).detach(); - } - }); - - std::promise prom; - auto fut = prom.get_future(); - _mission->start_mission_async([&prom](Mission::Result result) { - REQUIRE(Mission::Result::Success == result); - prom.set_value(); - }); - REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) == Failure::Result::Success); // We expect that a blind land is performed. - REQUIRE(poll_condition_with_timeout( - [this]() { - auto flight_mode = _telemetry->flight_mode(); - return flight_mode == Telemetry::FlightMode::Land; - }, std::chrono::seconds(90))); + wait_for_flight_mode(Telemetry::FlightMode::Land, std::chrono::seconds(30)); } void AutopilotTester::execute_mission_and_lose_mag() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { - std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + start_and_wait_for_first_mission_item(); - if (progress.current == 1) { - std::thread([this]() { - CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) - == Failure::Result::Success); - }).detach(); - } - }); - - std::promise prom; - auto fut = prom.get_future(); - _mission->start_mission_async([&prom](Mission::Result result) { - REQUIRE(Mission::Result::Success == result); - prom.set_value(); - }); - REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) == Failure::Result::Success); // We except the mission to continue without mag just fine. REQUIRE(poll_condition_with_timeout( @@ -296,31 +261,15 @@ void AutopilotTester::execute_mission_and_lose_mag() auto progress = _mission->mission_progress(); return progress.current == progress.total; }, std::chrono::seconds(90))); - } void AutopilotTester::execute_mission_and_lose_baro() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { - std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + start_and_wait_for_first_mission_item(); - if (progress.current == 1) { - std::thread([this]() { - CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) - == Failure::Result::Success); - }).detach(); - } - }); - - std::promise prom; - auto fut = prom.get_future(); - _mission->start_mission_async([&prom](Mission::Result result) { - REQUIRE(Mission::Result::Success == result); - prom.set_value(); - }); - REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) == Failure::Result::Success); // We except the mission to continue without baro just fine. REQUIRE(poll_condition_with_timeout( @@ -334,24 +283,9 @@ void AutopilotTester::execute_mission_and_get_baro_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { - std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + start_and_wait_for_first_mission_item(); - if (progress.current == 1) { - std::thread([this]() { - CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) - == Failure::Result::Success); - }).detach(); - } - }); - - std::promise prom; - auto fut = prom.get_future(); - _mission->start_mission_async([&prom](Mission::Result result) { - REQUIRE(Mission::Result::Success == result); - prom.set_value(); - }); - REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) == Failure::Result::Success); // We except the mission to continue with a stuck baro just fine. REQUIRE(poll_condition_with_timeout( @@ -365,24 +299,9 @@ void AutopilotTester::execute_mission_and_get_mag_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { - std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + start_and_wait_for_first_mission_item(); - if (progress.current == 1) { - std::thread([this]() { - CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) - == Failure::Result::Success); - }).detach(); - } - }); - - std::promise prom; - auto fut = prom.get_future(); - _mission->start_mission_async([&prom](Mission::Result result) { - REQUIRE(Mission::Result::Success == result); - prom.set_value(); - }); - REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) == Failure::Result::Success); // We except the mission to continue with a stuck mag just fine. REQUIRE(poll_condition_with_timeout( @@ -643,6 +562,55 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry: return pass; } +void AutopilotTester::start_and_wait_for_first_mission_item() +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) { + std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + + if (progress.current >= 1) { + _mission->subscribe_mission_progress(nullptr); + prom.set_value(); + } + }); + + REQUIRE(_mission->start_mission() == Mission::Result::Success); + + REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready); +} + +void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout) +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _telemetry->subscribe_flight_mode([&prom, flight_mode, this](Telemetry::FlightMode new_flight_mode) { + if (new_flight_mode == flight_mode) { + _telemetry->subscribe_flight_mode(nullptr); + prom.set_value(); + } + }); + + REQUIRE(fut.wait_for(timeout) == std::future_status::ready); +} + +void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout) +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _telemetry->subscribe_landed_state([&prom, landed_state, this](Telemetry::LandedState new_landed_state) { + if (new_landed_state == landed_state) { + _telemetry->subscribe_landed_state(nullptr); + prom.set_value(); + } + }); + + REQUIRE(fut.wait_for(timeout) == std::future_status::ready); +} + std::chrono::milliseconds AutopilotTester::adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms) { if (_info == nullptr) { diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 2f22733fe4..a798479dc5 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -126,6 +126,9 @@ private: bool ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos, float min_distance_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); + void start_and_wait_for_first_mission_item(); + void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout); + void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout); std::chrono::milliseconds adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms);