diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 09afdff439..504ae9d696 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -13,9 +13,10 @@ if(MAVSDK_FOUND) add_executable(mavsdk_tests test_main.cpp autopilot_tester.cpp - test_multicopter_offboard.cpp - test_multicopter_mission.cpp test_multicopter_failsafe.cpp + test_multicopter_mission.cpp + test_multicopter_offboard.cpp + test_multicopter_manual.cpp test_vtol_mission.cpp ) @@ -24,6 +25,7 @@ if(MAVSDK_FOUND) MAVSDK::mavsdk_action MAVSDK::mavsdk_failure MAVSDK::mavsdk_info + MAVSDK::mavsdk_manual_control MAVSDK::mavsdk_mission MAVSDK::mavsdk_offboard MAVSDK::mavsdk_param diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 0baf2b0622..3c170a22ce 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -52,6 +52,7 @@ void AutopilotTester::connect(const std::string uri) _action.reset(new Action(system)); _failure.reset(new Failure(system)); _info.reset(new Info(system)); + _manual_control.reset(new ManualControl(system)); _mission.reset(new Mission(system)); _offboard.reset(new Offboard(system)); _param.reset(new Param(system)); @@ -100,6 +101,11 @@ void AutopilotTester::check_home_within(float acceptance_radius_m) CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m)); } +void AutopilotTester::check_home_not_within(float min_distance_m) +{ + CHECK(ground_truth_horizontal_position_far_from(_home, min_distance_m)); +} + void AutopilotTester::set_takeoff_altitude(const float altitude_m) { CHECK(Action::Result::Success == _action->set_takeoff_altitude(altitude_m)); @@ -437,6 +443,68 @@ void AutopilotTester::check_mission_item_speed_above(int item_index, float min_s }); } +void AutopilotTester::fly_forward_in_posctl() +{ + // Send something to make sure RC is available. + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.f, 0.f) == ManualControl::Result::Success); + CHECK(_manual_control->start_position_control() == ManualControl::Result::Success); + + const unsigned manual_control_rate_hz = 20; + + // Climb up for 5 seconds + for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); + std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + } + + // Fly forward for 15 seconds + for (unsigned i = 0; i < 15 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + } + + // Descend until disarmed + for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success); + std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + + if (!_telemetry->in_air()) { + break; + } + } +} + +void AutopilotTester::fly_forward_in_altctl() +{ + // Send something to make sure RC is available. + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.f, 0.f) == ManualControl::Result::Success); + CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success); + + const unsigned manual_control_rate_hz = 20; + + // Climb up for 5 seconds + for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); + std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + } + + // Fly forward for 15 seconds + for (unsigned i = 0; i < 15 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + } + + // Descend until disarmed + for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success); + std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + + if (!_telemetry->in_air()) { + break; + } + } +} + void AutopilotTester::check_tracks_mission(float corridor_radius_m) { auto mission = _mission->download_mission(); @@ -521,21 +589,52 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry: global_current.latitude_deg = current_pos.latitude_deg; global_current.longitude_deg = current_pos.longitude_deg; LocalCoordinate local_pos = ct.local_from_global(global_current); - const double distance = sqrt(sq(local_pos.north_m) + sq(local_pos.east_m)); - const bool pass = distance < acceptance_radius_m; + const double distance_m = sqrt(sq(local_pos.north_m) + sq(local_pos.east_m)); + const bool pass = distance_m < acceptance_radius_m; if (!pass) { std::cout << "target_pos.lat: " << target_pos.latitude_deg << std::endl; std::cout << "target_pos.lon: " << target_pos.longitude_deg << std::endl; std::cout << "current.lat: " << current_pos.latitude_deg << std::endl; std::cout << "current.lon: " << current_pos.longitude_deg << std::endl; - std::cout << "Distance: " << distance << std::endl; + std::cout << "Distance: " << distance_m << std::endl; std::cout << "Acceptance radius: " << acceptance_radius_m << std::endl; } return pass; } +bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos, + float min_distance_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}); + + Telemetry::GroundTruth current_pos = _telemetry->ground_truth(); + CHECK(std::isfinite(current_pos.latitude_deg)); + CHECK(std::isfinite(current_pos.longitude_deg)); + GlobalCoordinate global_current; + global_current.latitude_deg = current_pos.latitude_deg; + global_current.longitude_deg = current_pos.longitude_deg; + LocalCoordinate local_pos = ct.local_from_global(global_current); + const double distance_m = sqrt(sq(local_pos.north_m) + sq(local_pos.east_m)); + const bool pass = distance_m > min_distance_m; + + if (!pass) { + std::cout << "target_pos.lat: " << target_pos.latitude_deg << std::endl; + std::cout << "target_pos.lon: " << target_pos.longitude_deg << std::endl; + std::cout << "current.lat: " << current_pos.latitude_deg << std::endl; + std::cout << "current.lon: " << current_pos.longitude_deg << std::endl; + std::cout << "Distance: " << distance_m << std::endl; + std::cout << "Min distance: " << min_distance_m << std::endl; + } + + return pass; +} + 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 4040e129e6..9258bff1ff 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -38,12 +38,14 @@ #include #include #include +#include #include #include #include #include #include "catch2/catch.hpp" #include +#include #include #include @@ -72,6 +74,7 @@ public: void wait_until_ready_local_position_only(); void store_home(); void check_home_within(float acceptance_radius_m); + void check_home_not_within(float min_distance_m); void set_takeoff_altitude(const float altitude_m); void set_height_source(HeightSource height_source); void arm(); @@ -93,6 +96,8 @@ public: 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 fly_forward_in_posctl(); + void fly_forward_in_altctl(); void request_ground_truth(); void check_mission_item_speed_above(int item_index, float min_speed_m_s); void check_tracks_mission(float corridor_radius_m = 1.0f); @@ -106,6 +111,7 @@ private: const mavsdk::geometry::CoordinateTransformation &ct); bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m); + 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); @@ -136,6 +142,7 @@ private: std::unique_ptr _action{}; std::unique_ptr _failure{}; std::unique_ptr _info{}; + std::unique_ptr _manual_control{}; std::unique_ptr _mission{}; std::unique_ptr _offboard{}; std::unique_ptr _param{}; diff --git a/test/mavsdk_tests/test_multicopter_manual.cpp b/test/mavsdk_tests/test_multicopter_manual.cpp new file mode 100644 index 0000000000..7e9f389083 --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_manual.cpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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" + + +TEST_CASE("Fly forward in position control", "[multicopter][vtol]") +{ + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.store_home(); + tester.arm(); + tester.fly_forward_in_posctl(); + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(20); + tester.wait_until_disarmed(until_disarmed_timeout); + tester.check_home_not_within(5.f); +} + +TEST_CASE("Fly forward in altitude control", "[multicopter][vtol]") +{ + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.store_home(); + tester.arm(); + tester.fly_forward_in_altctl(); + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(20); + tester.wait_until_disarmed(until_disarmed_timeout); + tester.check_home_not_within(5.f); +}