From b5048a3414b2329c044a068cb9cd72b9e0678e7e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 16:15:34 +0100 Subject: [PATCH] mavsdk_tests: add header and fix style --- test/mavsdk_tests/autopilot_tester.cpp | 265 ++++++++++-------- test/mavsdk_tests/autopilot_tester.h | 141 ++++++---- test/mavsdk_tests/test_main.cpp | 120 +++++--- .../mavsdk_tests/test_mission_multicopter.cpp | 94 ++++--- test/mavsdk_tests/test_mission_vtol.cpp | 54 +++- .../test_multicopter_offboard.cpp | 90 ++++-- 6 files changed, 477 insertions(+), 287 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 012037f5a9..5b39c9fbf7 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -1,3 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + #include "autopilot_tester.h" #include #include @@ -6,225 +39,229 @@ std::string connection_url {"udp://"}; void AutopilotTester::connect(const std::string uri) { - ConnectionResult ret = _mavsdk.add_any_connection(uri); - REQUIRE(ret == ConnectionResult::SUCCESS); + ConnectionResult ret = _mavsdk.add_any_connection(uri); + REQUIRE(ret == ConnectionResult::SUCCESS); - std::cout << "Waiting for system connect" << std::endl; - REQUIRE(poll_condition_with_timeout( - [this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25))); + std::cout << "Waiting for system connect" << std::endl; + REQUIRE(poll_condition_with_timeout( + [this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25))); - auto& system = _mavsdk.system(); + auto &system = _mavsdk.system(); - _telemetry.reset(new Telemetry(system)); - _action.reset(new Action(system)); - _mission.reset(new Mission(system)); - _offboard.reset(new Offboard(system)); + _telemetry.reset(new Telemetry(system)); + _action.reset(new Action(system)); + _mission.reset(new Mission(system)); + _offboard.reset(new Offboard(system)); } void AutopilotTester::wait_until_ready() { - std::cout << "Waiting for system to be ready" << std::endl; - CHECK(poll_condition_with_timeout( - [this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(20))); + std::cout << "Waiting for system to be ready" << std::endl; + CHECK(poll_condition_with_timeout( + [this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(20))); } void AutopilotTester::wait_until_ready_local_position_only() { - std::cout << "Waiting for system to be ready" << std::endl; - CHECK(poll_condition_with_timeout( - [this]() { - return - (_telemetry->health().gyrometer_calibration_ok && - _telemetry->health().accelerometer_calibration_ok && - _telemetry->health().magnetometer_calibration_ok && - _telemetry->health().level_calibration_ok && - _telemetry->health().local_position_ok); + std::cout << "Waiting for system to be ready" << std::endl; + CHECK(poll_condition_with_timeout( + [this]() { + return + (_telemetry->health().gyrometer_calibration_ok && + _telemetry->health().accelerometer_calibration_ok && + _telemetry->health().magnetometer_calibration_ok && + _telemetry->health().level_calibration_ok && + _telemetry->health().local_position_ok); }, std::chrono::seconds(20))); } void AutopilotTester::store_home() { - request_ground_truth(); - _home = get_ground_truth_position(); + request_ground_truth(); + _home = get_ground_truth_position(); } void AutopilotTester::check_home_within(float acceptance_radius_m) { - CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m)); + CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m)); } void AutopilotTester::set_takeoff_altitude(const float altitude_m) { - CHECK(Action::Result::SUCCESS == _action->set_takeoff_altitude(altitude_m)); - const auto result = _action->get_takeoff_altitude(); - CHECK(result.first == Action::Result::SUCCESS); - CHECK(result.second == Approx(altitude_m)); + CHECK(Action::Result::SUCCESS == _action->set_takeoff_altitude(altitude_m)); + const auto result = _action->get_takeoff_altitude(); + CHECK(result.first == Action::Result::SUCCESS); + CHECK(result.second == Approx(altitude_m)); } void AutopilotTester::arm() { - const auto result = _action->arm(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->arm(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::takeoff() { - const auto result = _action->takeoff(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->takeoff(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::land() { - const auto result = _action->land(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->land(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::transition_to_fixedwing() { - const auto result = _action->transition_to_fixedwing(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->transition_to_fixedwing(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::transition_to_multicopter() { - const auto result = _action->transition_to_multicopter(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->transition_to_multicopter(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::wait_until_disarmed() { - REQUIRE(poll_condition_with_timeout( - [this]() { return !_telemetry->armed(); }, std::chrono::seconds(60))); + REQUIRE(poll_condition_with_timeout( + [this]() { return !_telemetry->armed(); }, std::chrono::seconds(60))); } void AutopilotTester::wait_until_hovering() { - REQUIRE(poll_condition_with_timeout( - [this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(20))); + REQUIRE(poll_condition_with_timeout( + [this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(20))); } void AutopilotTester::prepare_square_mission(MissionOptions mission_options) { - const auto ct = get_coordinate_transformation(); + const auto ct = get_coordinate_transformation(); - std::vector> mission_items {}; - mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct)); - mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, mission_options, ct)); - mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct)); + std::vector> mission_items {}; + mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct)); + mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, + mission_options, ct)); + mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct)); - _mission->set_return_to_launch_after_mission(mission_options.rtl_at_end); + _mission->set_return_to_launch_after_mission(mission_options.rtl_at_end); - std::promise prom; - auto fut = prom.get_future(); + std::promise prom; + auto fut = prom.get_future(); - _mission->upload_mission_async(mission_items, [&prom](Mission::Result result) { - REQUIRE(Mission::Result::SUCCESS == result); - prom.set_value(); - }); + _mission->upload_mission_async(mission_items, [&prom](Mission::Result result) { + REQUIRE(Mission::Result::SUCCESS == result); + prom.set_value(); + }); - REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready); + REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready); } void AutopilotTester::execute_mission() { - std::promise prom; - auto fut = prom.get_future(); + std::promise prom; + auto fut = prom.get_future(); - _mission->start_mission_async([&prom](Mission::Result result) { - REQUIRE(Mission::Result::SUCCESS == result); - prom.set_value(); - }); + _mission->start_mission_async([&prom](Mission::Result result) { + REQUIRE(Mission::Result::SUCCESS == result); + prom.set_value(); + }); - // TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc. + // TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc. - REQUIRE(poll_condition_with_timeout( - [this]() { return _mission->mission_finished(); }, std::chrono::seconds(60))); + REQUIRE(poll_condition_with_timeout( + [this]() { return _mission->mission_finished(); }, std::chrono::seconds(60))); - REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); } CoordinateTransformation AutopilotTester::get_coordinate_transformation() { - const auto home = _telemetry->home_position(); - CHECK(std::isfinite(home.latitude_deg)); - CHECK(std::isfinite(home.longitude_deg)); - return CoordinateTransformation({home.latitude_deg, home.longitude_deg}); + const auto home = _telemetry->home_position(); + CHECK(std::isfinite(home.latitude_deg)); + CHECK(std::isfinite(home.longitude_deg)); + return CoordinateTransformation({home.latitude_deg, home.longitude_deg}); } std::shared_ptr AutopilotTester::create_mission_item( - const CoordinateTransformation::LocalCoordinate& local_coordinate, - const MissionOptions& mission_options, - const CoordinateTransformation& ct) + const CoordinateTransformation::LocalCoordinate &local_coordinate, + const MissionOptions &mission_options, + const CoordinateTransformation &ct) { - auto mission_item = std::make_shared(); - const auto pos_north = ct.global_from_local(local_coordinate); - mission_item->set_position(pos_north.latitude_deg, pos_north.longitude_deg); - mission_item->set_relative_altitude(mission_options.relative_altitude_m); - return mission_item; + auto mission_item = std::make_shared(); + const auto pos_north = ct.global_from_local(local_coordinate); + mission_item->set_position(pos_north.latitude_deg, pos_north.longitude_deg); + mission_item->set_relative_altitude(mission_options.relative_altitude_m); + return mission_item; } void AutopilotTester::execute_rtl() { - REQUIRE(Action::Result::SUCCESS == _action->return_to_launch()); + REQUIRE(Action::Result::SUCCESS == _action->return_to_launch()); } -void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius_m, std::chrono::seconds timeout_duration) +void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw &target, float acceptance_radius_m, + std::chrono::seconds timeout_duration) { - _offboard->set_position_ned(target); - REQUIRE(_offboard->start() == Offboard::Result::SUCCESS); - CHECK(poll_condition_with_timeout( - [=]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration)); - std::cout << "Target position reached" << std::endl; + _offboard->set_position_ned(target); + REQUIRE(_offboard->start() == Offboard::Result::SUCCESS); + CHECK(poll_condition_with_timeout( + [ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration)); + std::cout << "Target position reached" << std::endl; } void AutopilotTester::offboard_land() { - Offboard::VelocityNEDYaw land_velocity; - land_velocity.north_m_s = 0.0f; - land_velocity.east_m_s = 0.0f; - land_velocity.down_m_s = 1.0f; - land_velocity.yaw_deg = 0.0f; - _offboard->set_velocity_ned(land_velocity); + Offboard::VelocityNEDYaw land_velocity; + land_velocity.north_m_s = 0.0f; + land_velocity.east_m_s = 0.0f; + land_velocity.down_m_s = 1.0f; + land_velocity.yaw_deg = 0.0f; + _offboard->set_velocity_ned(land_velocity); } -bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m) +bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m) { - Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; - return sq(est_pos.north_m - target_pos.north_m) + - sq(est_pos.east_m - target_pos.east_m) + - sq(est_pos.down_m - target_pos.down_m) < sq(acceptance_radius_m); + Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; + return sq(est_pos.north_m - target_pos.north_m) + + sq(est_pos.east_m - target_pos.east_m) + + sq(est_pos.down_m - target_pos.down_m) < sq(acceptance_radius_m); } -bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m) +bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, + float acceptance_radius_m) { - Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; - return sq(est_pos.north_m - target_pos.north_m) + - sq(est_pos.east_m - target_pos.east_m) < sq(acceptance_radius_m); + Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; + return sq(est_pos.north_m - target_pos.north_m) + + sq(est_pos.east_m - target_pos.east_m) < sq(acceptance_radius_m); } void AutopilotTester::request_ground_truth() { - CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + 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(); + return _telemetry->ground_truth(); } -bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m) +bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, + float acceptance_radius_m) { - CHECK(std::isfinite(target_pos.latitude_deg)); - CHECK(std::isfinite(target_pos.longitude_deg)); - using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate; - using LocalCoordinate = CoordinateTransformation::LocalCoordinate; - CoordinateTransformation ct(GlobalCoordinate{target_pos.latitude_deg, target_pos.longitude_deg}); + CHECK(std::isfinite(target_pos.latitude_deg)); + CHECK(std::isfinite(target_pos.longitude_deg)); + using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate; + using LocalCoordinate = CoordinateTransformation::LocalCoordinate; + CoordinateTransformation ct(GlobalCoordinate{target_pos.latitude_deg, target_pos.longitude_deg}); - Telemetry::GroundTruth current_pos = _telemetry->ground_truth(); - 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}); + Telemetry::GroundTruth current_pos = _telemetry->ground_truth(); + 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}); - return sq(local_pos.north_m) + sq(local_pos.east_m) < sq(acceptance_radius_m); + return sq(local_pos.north_m) + sq(local_pos.east_m) < sq(acceptance_radius_m); } diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 3762eb0a0c..d520071e9e 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -1,3 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + #pragma once #include @@ -16,72 +49,76 @@ extern std::string connection_url; using namespace mavsdk; using namespace mavsdk::geometry; -class AutopilotTester { +class AutopilotTester +{ public: - struct MissionOptions { - double leg_length_m {20.0}; - double relative_altitude_m {10.0}; - bool rtl_at_end {false}; - }; + struct MissionOptions { + double leg_length_m {20.0}; + double relative_altitude_m {10.0}; + bool rtl_at_end {false}; + }; - void connect(const std::string uri); - void wait_until_ready(); - void wait_until_ready_local_position_only(); - void store_home(); - void check_home_within(float acceptance_radius_m); - void set_takeoff_altitude(const float altitude_m); - void arm(); - void takeoff(); - void land(); - void transition_to_fixedwing(); - void transition_to_multicopter(); - void wait_until_disarmed(); - void wait_until_hovering(); - void prepare_square_mission(MissionOptions mission_options); - void execute_mission(); - void execute_rtl(); - void offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius_m = 0.3f, - std::chrono::seconds timeout_duration = std::chrono::seconds(60)); - void offboard_land(); - void request_ground_truth(); + void connect(const std::string uri); + void wait_until_ready(); + void wait_until_ready_local_position_only(); + void store_home(); + void check_home_within(float acceptance_radius_m); + void set_takeoff_altitude(const float altitude_m); + void arm(); + void takeoff(); + void land(); + void transition_to_fixedwing(); + void transition_to_multicopter(); + void wait_until_disarmed(); + void wait_until_hovering(); + void prepare_square_mission(MissionOptions mission_options); + void execute_mission(); + void execute_rtl(); + void offboard_goto(const Offboard::PositionNEDYaw &target, float acceptance_radius_m = 0.3f, + std::chrono::seconds timeout_duration = std::chrono::seconds(60)); + void offboard_land(); + void request_ground_truth(); private: - mavsdk::geometry::CoordinateTransformation get_coordinate_transformation(); - std::shared_ptr create_mission_item( - const mavsdk::geometry::CoordinateTransformation::LocalCoordinate& local_coordinate, - const MissionOptions& mission_options, - const mavsdk::geometry::CoordinateTransformation& ct); - Telemetry::GroundTruth get_ground_truth_position(); + mavsdk::geometry::CoordinateTransformation get_coordinate_transformation(); + std::shared_ptr create_mission_item( + 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_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m); + 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_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m); - mavsdk::Mavsdk _mavsdk{}; - std::unique_ptr _telemetry{}; - std::unique_ptr _action{}; - std::unique_ptr _mission{}; - std::unique_ptr _offboard{}; + mavsdk::Mavsdk _mavsdk{}; + std::unique_ptr _telemetry{}; + std::unique_ptr _action{}; + std::unique_ptr _mission{}; + std::unique_ptr _offboard{}; - Telemetry::GroundTruth _home{NAN, NAN, NAN}; + Telemetry::GroundTruth _home{NAN, NAN, NAN}; }; template bool poll_condition_with_timeout( - std::function fun, std::chrono::duration duration) + std::function fun, std::chrono::duration duration) { - // We need millisecond resolution for sleeping. - const std::chrono::milliseconds duration_ms(duration); + // We need millisecond resolution for sleeping. + const std::chrono::milliseconds duration_ms(duration); - unsigned iteration = 0; - while (!fun()) { - std::this_thread::sleep_for(duration_ms / 10); - if (iteration++ >= 10) { - return false; - } - } - return true; + unsigned iteration = 0; + + while (!fun()) { + std::this_thread::sleep_for(duration_ms / 10); + + if (iteration++ >= 10) { + return false; + } + } + + return true; } inline float sq(float x) { return x * x; }; diff --git a/test/mavsdk_tests/test_main.cpp b/test/mavsdk_tests/test_main.cpp index 65753cef11..26c0f8ec5f 100644 --- a/test/mavsdk_tests/test_main.cpp +++ b/test/mavsdk_tests/test_main.cpp @@ -1,7 +1,35 @@ -// -// Multicopter mission test. -// -// Author: Julian Oes +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #define CATCH_CONFIG_RUNNER #include "catch2/catch.hpp" @@ -12,55 +40,59 @@ #include "autopilot_tester.h" -static void usage(const std::string& bin_name); -static void remove_argv(int& argc, char** argv, int pos); +static void usage(const std::string &bin_name); +static void remove_argv(int &argc, char **argv, int pos); -int main(int argc, char** argv) +int main(int argc, char **argv) { - for (int i = 0; i < argc; ++i) { - const std::string argv_string(argv[i]); + for (int i = 0; i < argc; ++i) { + const std::string argv_string(argv[i]); - if (argv_string == "-h") { - usage(argv[0]); - } + if (argv_string == "-h") { + usage(argv[0]); + } - if (argv_string == "--url") { - if (argc > i + 1) { - connection_url = argv[i+1]; - remove_argv(argc, argv, i); - remove_argv(argc, argv, i); - } else { - std::cerr << "No connection URL supplied" << std::endl; - usage(argv[0]); - return -1; - } - } - } + if (argv_string == "--url") { + if (argc > i + 1) { + connection_url = argv[i + 1]; + remove_argv(argc, argv, i); + remove_argv(argc, argv, i); - Catch::Session session; - const int catch_ret = session.applyCommandLine(argc, argv); - if (catch_ret != 0) { - return catch_ret; - } - return session.run(); + } else { + std::cerr << "No connection URL supplied" << std::endl; + usage(argv[0]); + return -1; + } + } + } + + Catch::Session session; + const int catch_ret = session.applyCommandLine(argc, argv); + + if (catch_ret != 0) { + return catch_ret; + } + + return session.run(); } -void usage(const std::string& bin_name) +void usage(const std::string &bin_name) { - std::cout << std::endl - << "Usage : " << bin_name << " [--url CONNECTION_URL] [catch2 arguments]" << std::endl - << "Connection URL format should be :" << std::endl - << " For TCP : tcp://[server_host][:server_port]" << std::endl - << " For UDP : udp://[bind_host][:bind_port]" << std::endl - << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl - << "For example, to connect to the simulator use URL: udp://:14540" << std::endl - << std::endl; + std::cout << std::endl + << "Usage : " << bin_name << " [--url CONNECTION_URL] [catch2 arguments]" << std::endl + << "Connection URL format should be :" << std::endl + << " For TCP : tcp://[server_host][:server_port]" << std::endl + << " For UDP : udp://[bind_host][:bind_port]" << std::endl + << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl + << "For example, to connect to the simulator use URL: udp://:14540" << std::endl + << std::endl; } -void remove_argv(int& argc, char** argv, int pos) +void remove_argv(int &argc, char **argv, int pos) { - for (int i = pos; i+1 < argc; ++i) { - argv[i] = argv[i+1]; - } - argv[--argc] = nullptr; + for (int i = pos; i + 1 < argc; ++i) { + argv[i] = argv[i + 1]; + } + + argv[--argc] = nullptr; } diff --git a/test/mavsdk_tests/test_mission_multicopter.cpp b/test/mavsdk_tests/test_mission_multicopter.cpp index 435d24fc10..c94a86bb1e 100644 --- a/test/mavsdk_tests/test_mission_multicopter.cpp +++ b/test/mavsdk_tests/test_mission_multicopter.cpp @@ -1,7 +1,35 @@ -// -// Multicopter mission test. -// -// Author: Julian Oes +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include #include @@ -13,39 +41,39 @@ TEST_CASE("Takeoff and Land", "[multicopter][vtol]") { - AutopilotTester tester; - tester.connect(connection_url); - tester.wait_until_ready(); - tester.arm(); - tester.takeoff(); - tester.wait_until_hovering(); - tester.land(); - tester.wait_until_disarmed(); + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.land(); + tester.wait_until_disarmed(); } TEST_CASE("Fly square Multicopter Missions", "[multicopter][vtol]") { - AutopilotTester tester; - tester.connect(connection_url); - tester.wait_until_ready(); + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); - SECTION("Mission including RTL") { - AutopilotTester::MissionOptions mission_options; - mission_options.rtl_at_end = true; - tester.prepare_square_mission(mission_options); - tester.arm(); - tester.execute_mission(); - tester.wait_until_disarmed(); - } + SECTION("Mission including RTL") { + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = true; + tester.prepare_square_mission(mission_options); + tester.arm(); + tester.execute_mission(); + tester.wait_until_disarmed(); + } - SECTION("Mission with manual RTL") { - AutopilotTester::MissionOptions mission_options; - mission_options.rtl_at_end = false; - tester.prepare_square_mission(mission_options); - tester.arm(); - tester.execute_mission(); - tester.wait_until_hovering(); - tester.execute_rtl(); - tester.wait_until_disarmed(); - } + SECTION("Mission with manual RTL") { + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = false; + tester.prepare_square_mission(mission_options); + tester.arm(); + tester.execute_mission(); + tester.wait_until_hovering(); + tester.execute_rtl(); + tester.wait_until_disarmed(); + } } diff --git a/test/mavsdk_tests/test_mission_vtol.cpp b/test/mavsdk_tests/test_mission_vtol.cpp index 5d8ff73747..91ee98f034 100644 --- a/test/mavsdk_tests/test_mission_vtol.cpp +++ b/test/mavsdk_tests/test_mission_vtol.cpp @@ -1,7 +1,35 @@ -// -// VTOL mission test. -// -// Author: Lorenz Meier +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include #include @@ -13,13 +41,13 @@ TEST_CASE("Takeoff and transition and RTL", "[vtol]") { - AutopilotTester tester; - tester.connect(connection_url); - tester.wait_until_ready(); - tester.arm(); - tester.takeoff(); - tester.wait_until_hovering(); - tester.transition_to_fixedwing(); - tester.execute_rtl(); - tester.wait_until_disarmed(); + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.transition_to_fixedwing(); + tester.execute_rtl(); + tester.wait_until_disarmed(); } diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index 678e9a61eb..0732e8a9a8 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -1,7 +1,35 @@ -// -// Multicopter mission test. -// -// Author: Julian Oes +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include #include @@ -13,35 +41,35 @@ TEST_CASE("Offboard takeoff and land", "[multicopter][offboard][nogps]") { - AutopilotTester tester; - Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; - tester.connect(connection_url); - tester.wait_until_ready_local_position_only(); - tester.store_home(); - tester.arm(); - tester.offboard_goto(takeoff_position, 0.5f); - tester.offboard_land(); - tester.wait_until_disarmed(); - tester.check_home_within(0.5f); + AutopilotTester tester; + Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; + tester.connect(connection_url); + tester.wait_until_ready_local_position_only(); + tester.store_home(); + tester.arm(); + tester.offboard_goto(takeoff_position, 0.5f); + tester.offboard_land(); + tester.wait_until_disarmed(); + tester.check_home_within(0.5f); } TEST_CASE("Offboard position control", "[multicopter][offboard][nogps]") { - AutopilotTester tester; - Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; - Offboard::PositionNEDYaw setpoint_1 {0.0f, 5.0f, -2.0f, 180.0f}; - Offboard::PositionNEDYaw setpoint_2 {5.0f, 5.0f, -4.0f, 180.0f}; - Offboard::PositionNEDYaw setpoint_3 {5.0f, 0.0f, -4.0f, 90.0f}; - tester.connect(connection_url); - tester.wait_until_ready_local_position_only(); - tester.store_home(); - tester.arm(); - tester.offboard_goto(takeoff_position, 0.5f); - tester.offboard_goto(setpoint_1, 1.0f); - tester.offboard_goto(setpoint_2, 1.0f); - tester.offboard_goto(setpoint_3, 1.0f); - tester.offboard_goto(takeoff_position, 0.2f); - tester.offboard_land(); - tester.wait_until_disarmed(); - tester.check_home_within(1.0f); + AutopilotTester tester; + Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; + Offboard::PositionNEDYaw setpoint_1 {0.0f, 5.0f, -2.0f, 180.0f}; + Offboard::PositionNEDYaw setpoint_2 {5.0f, 5.0f, -4.0f, 180.0f}; + Offboard::PositionNEDYaw setpoint_3 {5.0f, 0.0f, -4.0f, 90.0f}; + tester.connect(connection_url); + tester.wait_until_ready_local_position_only(); + tester.store_home(); + tester.arm(); + tester.offboard_goto(takeoff_position, 0.5f); + tester.offboard_goto(setpoint_1, 1.0f); + tester.offboard_goto(setpoint_2, 1.0f); + tester.offboard_goto(setpoint_3, 1.0f); + tester.offboard_goto(takeoff_position, 0.2f); + tester.offboard_land(); + tester.wait_until_disarmed(); + tester.check_home_within(1.0f); }