Add 'corridor check' which makes sure 3D tracking is along mission

test/mavsdk_tests/autopilot_tester.cpp

@@ -258,6 +258,64 @@ void AutopilotTester::check_mission_item_speed_above(int item_index, float min_s
 	});
 }
 
+void AutopilotTester::check_tracks_mission(float corridor_radius_m)
+{
+	auto mission = _mission->download_mission();
+	CHECK(mission.first == Mission::Result::Success);
+
+	std::vector<Mission::MissionItem> mission_items = mission.second.mission_items;
+	auto ct = get_coordinate_transformation();
+
+	auto get_local_mission_item = [mission_items, ct](int item_index) -> std::array<float, 3> {
+		using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate;
+		auto item = mission_items[item_index];
+		GlobalCoordinate global;
+		global.latitude_deg = item.latitude_deg;
+		global.longitude_deg = item.longitude_deg;
+		auto local = ct.local_from_global(global);
+		std::array<float, 3> res = {static_cast<float>(local.north_m), static_cast<float>(local.east_m), -item.relative_altitude_m};
+
+		return res;
+	};
+
+	_telemetry->set_rate_position_velocity_ned(5);
+	_telemetry->subscribe_position_velocity_ned([get_local_mission_item, corridor_radius_m,
+				this](Telemetry::PositionVelocityNed position_velocity_ned) {
+		auto progress = _mission->mission_progress();
+
+		if (progress.current > 0 && progress.current < progress.total) {
+			// Get shortest distance of current position to 3D line between previous and next waypoint
+			std::array<float, 3> current { position_velocity_ned.position.north_m,
+						       position_velocity_ned.position.east_m,
+						       position_velocity_ned.position.down_m };
+			std::array<float, 3> wp_prev = get_local_mission_item(progress.current - 1);
+			std::array<float, 3> wp_next = get_local_mission_item(progress.current);
+
+			// wp_dir_norm = (wp_next - wp_prev).normalize();
+			std::array<float, 3> wp_dir { wp_next[0] - wp_prev[0], wp_next[1] - wp_prev[1], wp_next[2] - wp_prev[2]};
+			float norm = std::sqrt(wp_dir[0] * wp_dir[0] + wp_dir[1] * wp_dir[1] + wp_dir[2] * wp_dir[2]);
+			std::array<float, 3> wp_dir_norm {wp_dir[0] / norm, wp_dir[1] / norm, wp_dir[2] / norm};
+
+			// travelled = current - wp_prev
+			std::array<float, 3> travelled {current[0] - wp_prev[0], current[1] - wp_prev[1], current[2] - wp_prev[2]};
+
+			// t = wp_dir_norm.dot(travelled);
+			float t = wp_dir_norm[0] * travelled[0] + wp_dir_norm[1] * travelled[1] + wp_dir_norm[2] * travelled[2];
+
+			// closest_on_line = wp_prev + t * wp_dir_norm;
+			std::array<float, 3> closest_on_line { wp_prev[0] + t *wp_dir_norm[0], wp_prev[1] + t *wp_dir_norm[1], wp_prev[2] + t *wp_dir_norm[2]};
+
+			// distance = (closest_on_line - current).norm();
+			std::array<float, 3> vec_to_line {closest_on_line[0] - current[0], closest_on_line[1] - current[1], closest_on_line[2] - current[2]};
+			float distance_to_trajectory = std::sqrt(vec_to_line[0] * vec_to_line[0] * vec_to_line[1] * vec_to_line[1] *
+						       vec_to_line[2] * vec_to_line[2]);
+
+			CHECK(distance_to_trajectory < corridor_radius_m);
+		}
+	});
+}
+
+
 void AutopilotTester::offboard_land()
 {
 	Offboard::VelocityNedYaw land_velocity;

test/mavsdk_tests/autopilot_tester.h

@@ -81,6 +81,7 @@ public:
 	void offboard_land();
 	void request_ground_truth();
 	void check_mission_item_speed_above(int item_index, float min_speed);
+	void check_tracks_mission(float corridor_radius_m = 1.0f);
 
 
 private:

test/mavsdk_tests/test_multicopter_mission.cpp

@@ -71,6 +71,7 @@ TEST_CASE("Fly square Multicopter Missions", "[multicopter][vtol]")
 		AutopilotTester::MissionOptions mission_options;
 		mission_options.rtl_at_end = false;
 		tester.prepare_square_mission(mission_options);
+		tester.check_tracks_mission(0.8f);
 		tester.arm();
 		tester.execute_mission();
 		tester.wait_until_hovering();
@@ -90,10 +91,10 @@ TEST_CASE("Fly straight Multicopter Mission", "[multicopter]")
 	mission_options.fly_through = true;
 	tester.prepare_straight_mission(mission_options);
 	tester.check_mission_item_speed_above(2, 4.5);
+	tester.check_tracks_mission(0.8f);
 	tester.arm();
 	tester.execute_mission();
 	tester.wait_until_hovering();
 	tester.execute_rtl();
 	tester.wait_until_disarmed();
-
 }