From f804b3516f24f61547a177fa2f4b21b55b11149a Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sun, 20 Mar 2022 12:41:31 +0100 Subject: [PATCH] Remove local planar vector --- src/lib/l1/ECL_L1_Pos_Controller.cpp | 13 ------------- src/lib/l1/ECL_L1_Pos_Controller.hpp | 13 ------------- 2 files changed, 26 deletions(-) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index 8e1880ab77..025c055287 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -374,19 +374,6 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) update_roll_setpoint(); } -Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2d &origin, const Vector2d &target) const -{ - /* this is an approximation for small angles, proposed by [2] */ - const double x_angle = math::radians(target(0) - origin(0)); - const double y_angle = math::radians(target(1) - origin(1)); - const double x_origin_cos = cos(math::radians(origin(0))); - - return Vector2f{ - static_cast(x_angle * CONSTANTS_RADIUS_OF_EARTH), - static_cast(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH), - }; -} - void ECL_L1_Pos_Controller::set_l1_period(float period) { _L1_period = period; diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp index a4ec91f2b3..8190e3e533 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.hpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp @@ -230,19 +230,6 @@ private: bool _has_guidance_updated = false; ///< this flag is set to true by any of the guidance methods. This flag has to be manually reset using has_guidance_updated_reset() - /** - * Convert a 2D vector from WGS84 to planar coordinates. - * - * This converts from latitude and longitude to planar - * coordinates with (0,0) being at the position of ref and - * returns a vector in meters towards wp. - * - * @param ref The reference position in WGS84 coordinates - * @param wp The point to convert to into the local coordinates, in WGS84 coordinates - * @return The vector in meters pointing from the reference position to the coordinates - */ - matrix::Vector2f get_local_planar_vector(const matrix::Vector2d &origin, const matrix::Vector2d &target) const; - /** * Update roll angle setpoint. This will also apply slew rate limits if set. *