mavsdk_tests: added posctl/altctl test

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

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) {

test/mavsdk_tests/autopilot_tester.h

@@ -38,12 +38,14 @@
 #include <mavsdk/plugins/action/action.h>
 #include <mavsdk/plugins/failure/failure.h>
 #include <mavsdk/plugins/info/info.h>
+#include <mavsdk/plugins/manual_control/manual_control.h>
 #include <mavsdk/plugins/mission/mission.h>
 #include <mavsdk/plugins/offboard/offboard.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 #include <mavsdk/plugins/param/param.h>
 #include "catch2/catch.hpp"
 #include <chrono>
+#include <iostream>
 #include <memory>
 #include <thread>
 
@@ -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<mavsdk::Action> _action{};
 	std::unique_ptr<mavsdk::Failure> _failure{};
 	std::unique_ptr<mavsdk::Info> _info{};
+	std::unique_ptr<mavsdk::ManualControl> _manual_control{};
 	std::unique_ptr<mavsdk::Mission> _mission{};
 	std::unique_ptr<mavsdk::Offboard> _offboard{};
 	std::unique_ptr<mavsdk::Param> _param{};

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);
+}