fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 13:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskSport: replace rotation by axis angle

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 5bd2afa7be
commit 6538a7e88c
1 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskSport.hpp
+7 -4
View File
@@ -43,6 +43,8 @@
#include "mathlib/mathlib.h"
#include <float.h>
using namespace matrix;
class FlightTaskSport : public FlightTaskManualPosition
{
public:
@@ -70,12 +72,13 @@ protected:
}
/* Scale to velocity using max velocity */
_vel_sp_xy = stick_xy * _vel_xy_max.get();
Vector2f vel_sp_xy = stick_xy * _vel_xy_max.get();
/* Rotate setpoint into local frame. */
matrix::Vector3f vel_sp { _vel_sp_xy(0), _vel_sp_xy(1), 0.0f };
vel_sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * vel_sp);
_vel_sp_xy = matrix::Vector2f(vel_sp(0), vel_sp(1));
matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw);
matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f));
_vel_sp(0) = vel_world(0);
_vel_sp(1) = vel_world(1);
}
private: