fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 02:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualSmoothing: change name of method

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 01073d36e5
commit f2f5f41641
2 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
+6 -6
View File
@@ -123,8 +123,8 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
* that the user demanded a direction change. * that the user demanded a direction change.
* The detection is done in body frame. */ * The detection is done in body frame. */
/* Rotate velocity setpoint into body frame */ /* Rotate velocity setpoint into body frame */
matrix::Vector2f vel_sp_heading = _getInHeadingFrame(vel_sp, yaw); matrix::Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
matrix::Vector2f vel_heading = _getInHeadingFrame(vel, yaw); matrix::Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
if (vel_sp_heading.length() > FLT_EPSILON) { if (vel_sp_heading.length() > FLT_EPSILON) {
vel_sp_heading.normalize(); vel_sp_heading.normalize();
@@ -250,7 +250,7 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
} }
matrix::Vector2f 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); 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 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::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)); matrix::Vector3f vec_world = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f));
return matrix::Vector2f(&vec_heading(0)); return matrix::Vector2f(&vec_world(0));
} }
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
+2 -2
View File
@@ -116,7 +116,7 @@ private:
void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
const float dt); const float dt);
void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt); void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
matrix::Vector2f _getInHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ; matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ;
matrix::Vector2f _getInWorldFrame(const matrix::Vector2f &vec, const float &yaw); matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw);
}; };