From f2f5f41641d838f4caa5cce1efeee95bded34f48 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 15 Feb 2018 09:35:24 +0100 Subject: [PATCH] FlightTaskManualSmoothing: change name of method --- .../FlightTasks/tasks/Utility/ManualSmoothingXY.cpp | 12 ++++++------ .../FlightTasks/tasks/Utility/ManualSmoothingXY.hpp | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp index c72dcc295a..ff9df574e2 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -123,8 +123,8 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V * that the user demanded a direction change. * The detection is done in body frame. */ /* Rotate velocity setpoint into body frame */ - matrix::Vector2f vel_sp_heading = _getInHeadingFrame(vel_sp, yaw); - matrix::Vector2f vel_heading = _getInHeadingFrame(vel, yaw); + matrix::Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw); + matrix::Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw); if (vel_sp_heading.length() > FLT_EPSILON) { vel_sp_heading.normalize(); @@ -250,7 +250,7 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt) } matrix::Vector2f -ManualSmoothingXY::_getInHeadingFrame(const matrix::Vector2f &vec, const float &yaw) +ManualSmoothingXY::_getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw) { matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw); @@ -259,10 +259,10 @@ ManualSmoothingXY::_getInHeadingFrame(const matrix::Vector2f &vec, const float & } matrix::Vector2f -ManualSmoothingXY::_getInWorldFrame(const matrix::Vector2f &vec, const float &yaw) +ManualSmoothingXY::_getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw) { matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw); - matrix::Vector3f vec_heading = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f)); - return matrix::Vector2f(&vec_heading(0)); + matrix::Vector3f vec_world = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f)); + return matrix::Vector2f(&vec_world(0)); } diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp index d021de1dcb..e3aa41df0a 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp @@ -116,7 +116,7 @@ private: void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, const float dt); void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt); - matrix::Vector2f _getInHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ; - matrix::Vector2f _getInWorldFrame(const matrix::Vector2f &vec, const float &yaw); + matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ; + matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw); };