fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 12:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualPosition: use axis angle to rotate into world frame

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent d22af4679d
commit 01073d36e5
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+4 -3
View File
@@ -74,9 +74,9 @@ void FlightTaskManualPosition::_scaleSticks()
_vel_sp_xy = stick_xy * _vel_xy_manual_max.get(); _vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
/* Rotate setpoint into local frame. */ /* Rotate setpoint into local frame. */
matrix::Vector3f vel_sp{_vel_sp_xy(0), _vel_sp_xy(1), 0.0f}; matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw);
vel_sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * vel_sp); matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), 0.0f));
_vel_sp_xy = matrix::Vector2f(vel_sp(0), vel_sp(1)); _vel_sp_xy = matrix::Vector2f(vel_world(0), vel_world(1));
} }
void FlightTaskManualPosition::_updateXYlock() void FlightTaskManualPosition::_updateXYlock()
@@ -90,6 +90,7 @@ void FlightTaskManualPosition::_updateXYlock()
_pos_sp_xy = matrix::Vector2f(&_position(0)); _pos_sp_xy = matrix::Vector2f(&_position(0));
} else if (!apply_brake) { } else if (!apply_brake) {
/* Don't use position setpoint */
_pos_sp_xy = _pos_sp_xy * NAN; _pos_sp_xy = _pos_sp_xy * NAN;
} }
} }