wip: squashed patch

test/mavsdk_tests/CMakeLists.txt

@@ -17,6 +17,7 @@ if(MAVSDK_FOUND)
         test_main.cpp
         autopilot_tester.cpp
         autopilot_tester_failure.cpp
+        autopilot_tester_follow_me.cpp
         test_multicopter_control_allocation.cpp
         test_multicopter_failure_injection.cpp
         test_multicopter_failsafe.cpp
@@ -24,6 +25,7 @@ if(MAVSDK_FOUND)
         test_multicopter_offboard.cpp
         test_multicopter_manual.cpp
         test_vtol_mission.cpp
+        test_multicopter_follow_me.cpp
     )
 
     target_link_libraries(mavsdk_tests

test/mavsdk_tests/autopilot_tester.cpp

@@ -37,6 +37,7 @@
 #include <future>
 #include <thread>
 #include <unistd.h>
+#include <cmath>
 
 std::string connection_url {"udp://"};
 std::optional<float> speed_factor {std::nullopt};
@@ -79,27 +80,24 @@ void AutopilotTester::connect(const std::string uri)
 
 void AutopilotTester::wait_until_ready()
 {
-	std::cout << time_str() << "Waiting for system to be ready" << std::endl;
+	std::cout << time_str() << "Waiting for system to be ready (system health ok & able to arm)" << std::endl;
+
+	// Wiat until the system is healthy
 	CHECK(poll_condition_with_timeout(
 	[this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30)));
 
-	// FIXME: workaround to prevent race between PX4 switching to Hold mode
-	// and us trying to arm and take off. If PX4 is not in Hold mode yet,
-	// our arming presumably triggers a failsafe in manual mode.
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-}
+	// Note: There is a known bug in MAVSDK (https://github.com/mavlink/MAVSDK/issues/1852),
+	// where `health_all_ok()` returning true doesn't actually mean vehicle is ready to accept
+	// global position estimate as valid (due to hysteresis). This needs to be fixed properly.
 
-void AutopilotTester::wait_until_ready_local_position_only()
-{
-	std::cout << time_str() << "Waiting for system to be ready" << std::endl;
+	// However, this is mitigated by the `is_armable` check below as a side effect, since
+	// when the vehicle considers global position to be valid, it will then allow arming
+	// if the COM_ARM_WO_GPS parameter is set to 0. For this case, switching to any mode
+	// relying on GPS after arming could be denied, and the test may fail.
+
+	// Wait until we can arm
 	CHECK(poll_condition_with_timeout(
-	[this]() {
-		return
-			(_telemetry->health().is_gyrometer_calibration_ok &&
-			 _telemetry->health().is_accelerometer_calibration_ok &&
-			 _telemetry->health().is_magnetometer_calibration_ok &&
-			 _telemetry->health().is_local_position_ok);
-	}, std::chrono::seconds(20)));
+	[this]() {	return _telemetry->health().is_armable;	}, std::chrono::seconds(20)));
 }
 
 void AutopilotTester::store_home()
@@ -534,6 +532,11 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 	});
 }
 
+void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float max_distance_m)
+{
+	CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m);
+}
+
 std::array<float, 3> AutopilotTester::get_current_position_ned()
 {
 	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();

test/mavsdk_tests/autopilot_tester.h

@@ -95,8 +95,12 @@ public:
 	~AutopilotTester();
 
 	void connect(const std::string uri);
+
+	/**
+	 * @brief Wait until vehicle's system status is healthy & is able to arm
+	 */
 	void wait_until_ready();
-	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);
@@ -133,6 +137,7 @@ public:
 	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.5f);
+	void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f);
 	void start_checking_altitude(const float max_deviation_m);
 	void stop_checking_altitude();
 
@@ -147,7 +152,41 @@ public:
 protected:
 	mavsdk::Param *getParams() const { return _param.get();}
 	mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
+	mavsdk::ManualControl *getManualControl() const { return _manual_control.get();}
 	std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
+	Telemetry::GroundTruth getHome()
+	{
+		// Check if home was stored before it is accessed
+		CHECK(_home.absolute_altitude_m != NAN);
+		CHECK(_home.latitude_deg != NAN);
+		CHECK(_home.longitude_deg != NAN);
+		return _home;
+	}
+
+	template<typename Rep, typename Period>
+	void sleep_for(std::chrono::duration<Rep, Period> duration)
+	{
+		const std::chrono::microseconds duration_us(duration);
+
+		if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
+
+			const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
+
+			while (true) {
+				// Hopefully this is often enough not to have PX4 time out on us.
+				std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+				const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
+
+				if (elapsed_time_us > duration_us.count()) {
+					return;
+				}
+			}
+
+		} else {
+			std::this_thread::sleep_for(duration);
+		}
+	}
 
 private:
 	mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
@@ -169,6 +208,12 @@ private:
 
 	void report_speed_factor();
 
+	/**
+	 * @brief Continue polling until condition returns true or we have a timeout
+	 *
+	 * @param fun Boolean returning function. When true, the polling terminates.
+	 * @param duration Timeout for polling in `std::chrono::` time unit
+	 */
 	template<typename Rep, typename Period>
 	bool poll_condition_with_timeout(
 		std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)
@@ -218,30 +263,7 @@ private:
 		return true;
 	}
 
-	template<typename Rep, typename Period>
-	void sleep_for(std::chrono::duration<Rep, Period> duration)
-	{
-		const std::chrono::microseconds duration_us(duration);
 
-		if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
-
-			const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
-
-			while (true) {
-				// Hopefully this is often enough not to have PX4 time out on us.
-				std::this_thread::sleep_for(std::chrono::milliseconds(1));
-
-				const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
-
-				if (elapsed_time_us > duration_us.count()) {
-					return;
-				}
-			}
-
-		} else {
-			std::this_thread::sleep_for(duration);
-		}
-	}
 
 	mavsdk::Mavsdk _mavsdk{};
 	std::unique_ptr<mavsdk::Action> _action{};

test/mavsdk_tests/autopilot_tester_follow_me.cpp

@@ -0,0 +1,453 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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_follow_me.h"
+
+// #include <mavsdk/plugins/follow_me/follow_me.h>
+
+#include "math_helpers.h"
+#include <iostream>
+#include <future>
+#include <thread>
+#include <unistd.h>
+#include <cmath>
+#include <random>
+
+
+FollowTargetSimulator::FollowTargetSimulator(std::array<float, 3> initial_position_ned,
+		mavsdk::Telemetry::GroundTruth home) :
+	_position_ned(initial_position_ned), _home(home)
+{
+	_velocity_ned[0] = 0.0f;
+	_velocity_ned[1] = 0.0f;
+	_velocity_ned[2] = 0.0f;
+}
+
+FollowTargetSimulator::~FollowTargetSimulator()
+{
+
+}
+
+void FollowTargetSimulator::update(float delta_t_s)
+{
+	const float velocity_m_s = 2.0;
+
+	_velocity_ned[0] = velocity_m_s;
+	_velocity_ned[1] = 0.0;
+
+	_position_ned[0] += _velocity_ned[0] * delta_t_s;
+	_position_ned[1] += _velocity_ned[1] * delta_t_s;
+	_position_ned[2] += _velocity_ned[2] * delta_t_s;
+
+	_udpate_count++;
+}
+
+std::array<double, 3> FollowTargetSimulator::get_position_global(bool add_noise)
+{
+	std::array<float, 3> pos_ned = _position_ned;
+
+	if (add_noise) {
+		unsigned seed = _udpate_count;
+		std::default_random_engine generator(seed);
+		std::normal_distribution<double> distribution(0.0, 1.0);
+		pos_ned[0] += distribution(generator);
+		pos_ned[1] += distribution(generator);
+		pos_ned[2] += distribution(generator);
+	}
+
+	CHECK(std::isfinite(_home.latitude_deg));
+	CHECK(std::isfinite(_home.longitude_deg));
+	const auto ct = CoordinateTransformation({_home.latitude_deg, _home.longitude_deg});
+
+	mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate{pos_ned[0], pos_ned[1]};
+	mavsdk::geometry::CoordinateTransformation::GlobalCoordinate global_coordinate = ct.global_from_local(local_coordinate);
+	std::array<double, 3> global_pos{global_coordinate.latitude_deg, global_coordinate.longitude_deg, pos_ned[2] + _home.absolute_altitude_m};
+	return global_pos;
+}
+
+std::array<float, 3> FollowTargetSimulator::get_position_ned(bool add_noise)
+{
+
+	std::array<float, 3> pos_ned = _position_ned;
+
+	if (add_noise) {
+		unsigned seed = _udpate_count;
+		std::default_random_engine generator(seed);
+		std::normal_distribution<double> distribution(0.0, pos_noise_std);
+		pos_ned[0] += distribution(generator);
+		pos_ned[1] += distribution(generator);
+		pos_ned[2] += distribution(generator);
+	}
+
+	return pos_ned;
+}
+
+std::array<float, 3> FollowTargetSimulator::get_velocity_ned_noisy()
+{
+	return get_velocity_ned(true);
+}
+
+std::array<float, 3> FollowTargetSimulator::get_velocity_ned_ground_truth()
+{
+	return get_velocity_ned(false);
+}
+
+std::array<float, 3> FollowTargetSimulator::get_velocity_ned(bool add_noise)
+{
+	std::array<float, 3> vel_ned = _velocity_ned;
+
+	if (add_noise) {
+		unsigned seed = _udpate_count;
+		std::default_random_engine generator(seed);
+		std::normal_distribution<double> distribution(0.0, vel_noise_std);
+
+		vel_ned[0] += distribution(generator);
+		vel_ned[1] += distribution(generator);
+		vel_ned[2] += distribution(generator);
+	}
+
+	return vel_ned;
+}
+
+std::array<float, 3> FollowTargetSimulator::get_position_ned_noisy()
+{
+	return get_position_ned(true);
+}
+
+std::array<float, 3> FollowTargetSimulator::get_position_ground_truth_ned()
+{
+	return get_position_ned(false);
+}
+
+std::array<double, 3> FollowTargetSimulator::get_position_global_noisy()
+{
+	return get_position_global(true);
+}
+
+std::array<double, 3> FollowTargetSimulator::get_position_global_ground_truth()
+{
+	return get_position_global(false);
+}
+
+void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
+		std::array<float, 3> target_pos_ned, float tolerance)
+{
+	// This check assumes that the target is travelling straight on the x-axis
+	const float x_dist_to_target = target_pos_ned[0] - drone_pos_ned[0];
+	const float y_dist_to_target = target_pos_ned[1] - drone_pos_ned[1];
+
+	switch (config.follow_direction) {
+	case FollowMe::Config::FollowDirection::None:
+		CHECK(x_dist_to_target < tolerance);
+		CHECK(x_dist_to_target > -tolerance);
+		CHECK(y_dist_to_target < tolerance);
+		CHECK(y_dist_to_target > -tolerance);
+		break;
+
+	case FollowMe::Config::FollowDirection::Behind:
+		CHECK(drone_pos_ned[0] < target_pos_ned[0]);
+		CHECK(y_dist_to_target < tolerance);
+		CHECK(y_dist_to_target > -tolerance);
+		break;
+
+	case FollowMe::Config::FollowDirection::Front:
+		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
+		CHECK(y_dist_to_target < tolerance);
+		CHECK(y_dist_to_target > -tolerance);
+		break;
+
+	case FollowMe::Config::FollowDirection::FrontRight:
+		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
+		CHECK(drone_pos_ned[1] > target_pos_ned[1]);
+		break;
+
+	case FollowMe::Config::FollowDirection::FrontLeft:
+		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
+		CHECK(drone_pos_ned[1] < target_pos_ned[1]);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void AutopilotTesterFollowMe::connect(const std::string uri)
+{
+	AutopilotTester::connect(uri);
+
+	auto system = get_system();
+	_follow_me.reset(new FollowMe(system));
+}
+
+
+void AutopilotTesterFollowMe::straight_line_test(const float altitude_m, const bool stream_velocity)
+{
+	const unsigned location_update_rate = 1;
+	const float position_tolerance = 6.0f;
+
+	// Start with simulated target on the same plane as drone's home position
+	std::array<float, 3> start_location_ned = get_current_position_ned();
+	FollowTargetSimulator target_simulator(start_location_ned, getHome());
+
+	// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
+	// right".
+	FollowMe::Config config;
+	config.min_height_m = altitude_m;
+	config.follow_distance_m = 8.0f;
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	// Start Follow Me
+	CHECK(FollowMe::Result::Success ==  _follow_me->start());
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	// task loop
+	bool target_moving = false;
+	bool perform_checks = false;
+
+	for (unsigned i = 0; i < 75 * location_update_rate; ++i) {
+		std::array<float, 3> target_pos_ned_ground_truth = target_simulator.get_position_ground_truth_ned();
+		std::array<float, 3> position_ned = get_current_position_ned();
+		const float distance_to_target = norm(diff(target_pos_ned_ground_truth, position_ned));
+
+		// poor-man's state machine
+		if (i < 5) {
+			// Stream target location without moving
+
+		} else if (i == 5) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 5 && i < 15) {
+			// Move target and wait for steady state of drone
+			target_moving = true;
+
+		} else if (i >= 15 && i < 20) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+
+		} else if (i == 20) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::Front;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 20 && i < 30) {
+			// Move target and wait for steady state of drone
+
+		} else if (i >= 30 && i < 35) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+
+		} else if (i == 35) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::FrontRight;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 35 && i < 45) {
+			// Move target and wait for steady state of drone
+
+
+		} else if (i >= 45 && i < 55) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+
+		} else if (i == 55) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::FrontLeft;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 55 && i < 65) {
+			// Move target and wait for steady state of drone
+
+		} else if (i >= 65 && i < 75) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+		}
+
+		if (target_moving) {
+			target_simulator.update(1.0f / location_update_rate);
+		}
+
+		if (perform_checks) {
+			check_current_altitude(altitude_m);
+			CHECK(distance_to_target <= config.follow_distance_m + position_tolerance);
+			CHECK(distance_to_target >= config.follow_distance_m - position_tolerance);
+			target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth, position_tolerance);
+		}
+
+		// Construct follow-me message
+		std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
+
+		FollowMe::TargetLocation target_location{};
+		target_location.latitude_deg = global_coordinate[0];
+		target_location.longitude_deg = global_coordinate[1];
+		target_location.absolute_altitude_m = global_coordinate[2];
+
+		if (stream_velocity) {
+			std::array<float, 3> target_vel_ned = target_simulator.get_velocity_ned_noisy();
+			target_location.velocity_x_m_s = target_vel_ned[0];
+			target_location.velocity_y_m_s = target_vel_ned[1];
+			target_location.velocity_z_m_s = target_vel_ned[2];
+
+		} else {
+			target_location.velocity_x_m_s = NAN;
+			target_location.velocity_y_m_s = NAN;
+			target_location.velocity_z_m_s = NAN;
+		}
+
+
+		// Send message and check result
+		CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
+
+		sleep_for(std::chrono::milliseconds(1000 / location_update_rate));
+	}
+
+	CHECK(FollowMe::Result::Success ==  _follow_me->stop());
+}
+
+void AutopilotTesterFollowMe::stream_velocity_only()
+{
+	const unsigned loop_update_rate = 1;
+	const float position_tolerance = 4.0f;
+
+	// Configure follow-me
+	FollowMe::Config config;
+	config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+	CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	// Start Follow Me
+	CHECK(FollowMe::Result::Success ==  _follow_me->start());
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	std::array<float, 3> drone_initial_pos = get_current_position_ned();
+
+	// Start streaming velocity only. The drone should not move.
+	for (unsigned i = 0; i < 15 * loop_update_rate; i++) {
+		FollowMe::TargetLocation target_location{};
+		target_location.latitude_deg = NAN;
+		target_location.longitude_deg = NAN;
+		target_location.absolute_altitude_m = NAN;
+		target_location.velocity_x_m_s = 2.0f;
+		target_location.velocity_y_m_s = 1.0f;
+		target_location.velocity_z_m_s = 0.5f;
+
+		// Send message and check result
+		CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
+
+		sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
+	}
+
+	// Check that drone is still close to initial position and has not moved much
+	std::array<float, 3> drone_final_pos = get_current_position_ned();
+	const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos));
+	CHECK(distance_travelled < position_tolerance);
+}
+
+void AutopilotTesterFollowMe::rc_override_test(const float altitude_m)
+{
+	const unsigned loop_update_rate = 50;
+	const float position_tolerance = 4.0f;
+
+	// Start with simulated target on the same plane as drone's home position
+	std::array<float, 3> start_location_ned = get_current_position_ned();
+	FollowTargetSimulator target_simulator(start_location_ned, getHome());
+
+	// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
+	// right".
+	FollowMe::Config config;
+	config.min_height_m = altitude_m;
+	config.follow_distance_m = 8.0f;
+	config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+	CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+	// task loop
+	std::array<float, 3> drone_initial_pos;
+
+	for (unsigned i = 0; i <= 30 * loop_update_rate; ++i) {
+		// Start streaming target data after x seconds to provide RC before switching to the flight task
+		bool stream_follow_me_data = (i > 7 * loop_update_rate);
+
+		// Deflect a stick for a short time only
+		bool deflect_rc_sticks = (i > 10 * loop_update_rate && i <= 11 * loop_update_rate);
+
+		// Switch to follow-me at this instant
+		if (i == 5 * loop_update_rate) {
+			// Start Follow Me
+			CHECK(FollowMe::Result::Success ==  _follow_me->start());
+		}
+
+		// After approximately 10 seconds we would expect the drone to have stopped because of the RC stick input
+		if (i == 20 * loop_update_rate) {
+			drone_initial_pos = get_current_position_ned();
+		}
+
+		if (stream_follow_me_data) {
+			target_simulator.update(1.0f / loop_update_rate);
+			std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
+			FollowMe::TargetLocation target_location{};
+			target_location.latitude_deg = global_coordinate[0];
+			target_location.longitude_deg = global_coordinate[1];
+			target_location.absolute_altitude_m = global_coordinate[2];
+			target_location.velocity_x_m_s = NAN;
+			target_location.velocity_y_m_s = NAN;
+			target_location.velocity_z_m_s = NAN;
+			_follow_me->set_target_location(target_location);
+		}
+
+		if (deflect_rc_sticks) {
+			CHECK(getManualControl()->set_manual_control_input(1.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else {
+			CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+		}
+
+		sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
+	}
+
+	std::array<float, 3> drone_final_pos = get_current_position_ned();
+	const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos));
+	CHECK(distance_travelled < position_tolerance);
+}

test/mavsdk_tests/autopilot_tester_follow_me.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "autopilot_tester.h"
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/follow_me/follow_me.h>
+
+
+// Simulated a target moving on a straight line
+class FollowTargetSimulator
+{
+public:
+	FollowTargetSimulator(std::array<float, 3> initial_position_ned, mavsdk::Telemetry::GroundTruth home);
+	~FollowTargetSimulator();
+
+	// Integrate simulator by one time step
+	// This moves the target on a line
+	void update(float delta_t_s);
+
+	// Retrieve noisy version of position state in NED coordinate frame
+	// The noise is deterministic and changes whenever the update() function is called
+	std::array<float, 3> get_position_ned_noisy();
+
+	// Retrieve ground truth of position state in NED coordinate frame
+	std::array<float, 3> get_position_ground_truth_ned();
+
+	// Retrieve noisy version of velocity state in NED coordinate frame
+	// The noise is deterministic and changes whenever the update() function is called
+	std::array<float, 3> get_velocity_ned_noisy();
+
+	// Retrieve ground truth of velocity state in NED coordinate frame
+	std::array<float, 3> get_velocity_ned_ground_truth();
+
+	// Retrieve noisy version of position state in global coordinate frame (lat, lon, alt)
+	// The noise is deterministic and changes whenever the update() function is called
+	std::array<double, 3> get_position_global_noisy();
+
+	// Retrieve ground truth of position state in global coordinate frame (lat, lon, alt)
+	std::array<double, 3> get_position_global_ground_truth();
+
+	// Run checks whether the drone has the correct angle towards the target, specified by the follow-me configuration
+	void check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
+				std::array<float, 3> target_pos_ned, float tolerance);
+
+private:
+	// Retrieve estimate with the option to add deterministic gaussian noise
+	//
+	// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
+	std::array<double, 3> get_position_global(bool add_noise);
+
+	// Retrieve estimate with the option to add deterministic gaussian noise
+	//
+	// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
+	std::array<float, 3> get_position_ned(bool add_noise);
+
+	// Retrieve estimate with the option to add deterministic gaussian noise
+	//
+	// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
+	std::array<float, 3> get_velocity_ned(bool add_noise);
+
+	std::array<float, 3> _position_ned;
+	std::array<float, 3> _velocity_ned;
+	mavsdk::Telemetry::GroundTruth _home;
+	size_t _udpate_count = 0;
+
+	const double pos_noise_std = 0.3;
+	const double vel_noise_std = 0.1;
+
+	std::unique_ptr<mavsdk::Telemetry> _telemetry{};
+};
+
+class AutopilotTesterFollowMe : public AutopilotTester
+{
+public:
+	AutopilotTesterFollowMe() = default;
+	~AutopilotTesterFollowMe() = default;
+	void connect(const std::string uri);
+
+	void straight_line_test(const float altitude_m, const bool stream_velocity);
+
+	void stream_velocity_only();
+
+	void rc_override_test(const float altitude_m);
+
+private:
+	std::unique_ptr<mavsdk::FollowMe> _follow_me{};
+};

test/mavsdk_tests/test_multicopter_follow_me.cpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 <chrono>
+
+#include "autopilot_tester_follow_me.h"
+
+
+
+TEST_CASE("Follow target - Position streaming", "[multicopter]")
+{
+	const bool stream_velocity = false;
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.straight_line_test(takeoff_altitude, stream_velocity);
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
+{
+	const bool stream_velocity = true;
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.straight_line_test(takeoff_altitude, stream_velocity);
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
+{
+	// Streaming only velocity should not work, the drone should not move.
+	// There needs to be at least one position message for follow-me
+	// to be able to integrate velocity.
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.stream_velocity_only();
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+	tester.check_home_within(1.0f);
+}
+
+TEST_CASE("Follow target - Manual takeover", "[multicopter]")
+{
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.rc_override_test(takeoff_altitude);
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Follow target - Spamming duplicate messages", "[multicopter]")
+{
+
+}

test/mavsdk_tests/test_vtol_mission.cpp

@@ -39,7 +39,7 @@ TEST_CASE("Fly VTOL mission", "[vtol]")
 	AutopilotTester tester;
 	tester.connect(connection_url);
 	tester.wait_until_ready();
-	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_allmend.plan");
 	tester.arm();
 	tester.execute_mission_raw();
 	tester.wait_until_disarmed();

test/mavsdk_tests/vtol_mission_allmend.plan

@@ -0,0 +1,200 @@
+{
+    "fileType": "Plan",
+    "geoFence": {
+        "circles": [
+        ],
+        "polygons": [
+        ],
+        "version": 2
+    },
+    "groundStation": "QGroundControl",
+    "mission": {
+        "cruiseSpeed": 15,
+        "firmwareType": 12,
+        "globalPlanAltitudeMode": 1,
+        "hoverSpeed": 5,
+        "items": [
+            {
+                "AMSLAltAboveTerrain": 20,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 84,
+                "doJumpId": 1,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35575503646861,
+                    8.52080660767436,
+                    20
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 20,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 2,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35537212,
+                    8.52207767,
+                    20
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 3,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35349688,
+                    8.52355825,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 4,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.3528936,
+                    8.52294671,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 5,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35230484,
+                    8.52030742,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 6,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35294448,
+                    8.51882684,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 7,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35375855,
+                    8.52002847,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "autoContinue": true,
+                "command": 189,
+                "doJumpId": 8,
+                "frame": 2,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 0,
+                "Altitude": 0,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 85,
+                "doJumpId": 9,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    20,
+                    null,
+                    47.3557210047974,
+                    8.518762469830165,
+                    0
+                ],
+                "type": "SimpleItem"
+            }
+        ],
+        "plannedHomePosition": [
+            47.3557317,
+            8.5187574,
+            418
+        ],
+        "vehicleType": 20,
+        "version": 2
+    },
+    "rallyPoints": {
+        "points": [
+        ],
+        "version": 2
+    },
+    "version": 1
+}