From e6c5771c59800f63ba531634fba25457b6be2c53 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Thu, 9 Jul 2020 17:39:13 +0200 Subject: [PATCH] Add 'corridor check' which makes sure 3D tracking is along mission --- test/mavsdk_tests/autopilot_tester.cpp | 58 +++++++++++++++++++ test/mavsdk_tests/autopilot_tester.h | 1 + .../mavsdk_tests/test_multicopter_mission.cpp | 3 +- 3 files changed, 61 insertions(+), 1 deletion(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 109daa3745..8db79af630 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/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_items = mission.second.mission_items; + auto ct = get_coordinate_transformation(); + + auto get_local_mission_item = [mission_items, ct](int item_index) -> std::array { + 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 res = {static_cast(local.north_m), static_cast(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 current { position_velocity_ned.position.north_m, + position_velocity_ned.position.east_m, + position_velocity_ned.position.down_m }; + std::array wp_prev = get_local_mission_item(progress.current - 1); + std::array wp_next = get_local_mission_item(progress.current); + + // wp_dir_norm = (wp_next - wp_prev).normalize(); + std::array 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 wp_dir_norm {wp_dir[0] / norm, wp_dir[1] / norm, wp_dir[2] / norm}; + + // travelled = current - wp_prev + std::array 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 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 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; diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index ca3651228f..45147588e4 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/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: diff --git a/test/mavsdk_tests/test_multicopter_mission.cpp b/test/mavsdk_tests/test_multicopter_mission.cpp index c3e86139c0..73749f9006 100644 --- a/test/mavsdk_tests/test_multicopter_mission.cpp +++ b/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(); - }