diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 337a9f5932..f65cc8efc0 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -32,64 +32,12 @@ ****************************************************************************/ #include "autopilot_tester.h" +#include "math_helpers.h" #include #include std::string connection_url {"udp://"}; -namespace -{ -std::array get_local_mission_item(const Mission::MissionItem &item, const CoordinateTransformation &ct) -{ - using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate; - GlobalCoordinate global; - global.latitude_deg = item.latitude_deg; - global.longitude_deg = item.longitude_deg; - auto local = ct.local_from_global(global); - return {static_cast(local.north_m), static_cast(local.east_m), -item.relative_altitude_m}; -} - -float norm(const std::array &vec) -{ - return std::sqrt(sq(vec[0]) + sq(vec[1]) + sq(vec[2])); -} - -float dot(const std::array &vec1, const std::array &vec2) -{ - return vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2]; -} - -std::array diff(const std::array &vec1, const std::array &vec2) -{ - return {vec1[0] - vec2[0], vec1[1] - vec2[1], vec1[2] - vec2[2]}; -} - -std::array normalized(const std::array &vec) -{ - float n = norm(vec); - - if (n > 1e-6f) { - return {vec[0] / n, vec[1] / n, vec[2] / n}; - - } else { - return {0, 0, 0}; - } -} - -float point_to_line_distance(const std::array &point, const std::array &line_start, - const std::array &line_end) -{ - std::array norm_dir = normalized(diff(line_end, line_start)); - float t = dot(norm_dir, diff(point, line_start)); - - // closest_on_line = line_start + t * norm_dir; - std::array closest_on_line { line_start[0] + t *norm_dir[0], line_start[1] + t *norm_dir[1], line_start[2] + t *norm_dir[2]}; - - return norm(diff(closest_on_line, point)); -} - -} - void AutopilotTester::connect(const std::string uri) { ConnectionResult ret = _mavsdk.add_any_connection(uri); @@ -498,8 +446,8 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m) 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(mission_items[progress.current - 1], ct); - std::array wp_next = get_local_mission_item(mission_items[progress.current], ct); + std::array wp_prev = get_local_mission_item(mission_items[progress.current - 1], ct); + std::array wp_next = get_local_mission_item(mission_items[progress.current], ct); float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next); diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 10f3b6c58e..f6d391a7c7 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -132,8 +132,6 @@ private: return true; } - static float sq(float x) { return x * x; }; - mavsdk::Mavsdk _mavsdk{}; std::unique_ptr _action{}; std::unique_ptr _failure{}; diff --git a/test/mavsdk_tests/math_helpers.h b/test/mavsdk_tests/math_helpers.h new file mode 100644 index 0000000000..34817381ef --- /dev/null +++ b/test/mavsdk_tests/math_helpers.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * 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 + +template +std::array get_local_mission_item(const Mission::MissionItem &item, const CoordinateTransformation &ct) +{ + using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate; + GlobalCoordinate global; + global.latitude_deg = item.latitude_deg; + global.longitude_deg = item.longitude_deg; + auto local = ct.local_from_global(global); + return {static_cast(local.north_m), static_cast(local.east_m), -item.relative_altitude_m}; +} + +template +T sq(T x) +{ + return x * x; +} + +template +T norm(const std::array &vec) +{ + return std::sqrt(sq(vec[0]) + sq(vec[1]) + sq(vec[2])); +} + +template +T dot(const std::array &vec1, const std::array &vec2) +{ + return vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2]; +} + + +template +std::array diff(const std::array &vec1, const std::array &vec2) +{ + return {vec1[0] - vec2[0], vec1[1] - vec2[1], vec1[2] - vec2[2]}; +} + +template +std::array normalized(const std::array &vec) +{ + T n = norm(vec); + + if (n > 1e-6f) { + return {vec[0] / n, vec[1] / n, vec[2] / n}; + + } else { + return {0, 0, 0}; + } +} + +template +T point_to_line_distance(const std::array &point, const std::array &line_start, + const std::array &line_end) +{ + std::array norm_dir = normalized(diff(line_end, line_start)); + T t = dot(norm_dir, diff(point, line_start)); + + // closest_on_line = line_start + t * norm_dir; + std::array closest_on_line { line_start[0] + t *norm_dir[0], line_start[1] + t *norm_dir[1], line_start[2] + t *norm_dir[2]}; + + return norm(diff(closest_on_line, point)); +}