From 017576262c0074bb9473e45b16ba0c35f99f7562 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 21 Feb 2018 10:53:40 +0100 Subject: [PATCH] FlightTaskManual: replace yaw with yaw_sp for rotation --- .../FlightTasks/tasks/FlightTaskManualAltitude.cpp | 4 ++-- .../FlightTasks/tasks/FlightTaskManualPosition.cpp | 11 +++++------ .../FlightTasks/tasks/FlightTaskManualStabilized.cpp | 3 ++- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index ac547dc4aa..65bdb234bf 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -94,8 +94,8 @@ void FlightTaskManualAltitude::_updateSetpoints() * thrust along xy is required. The maximum thrust along xy depends on the thrust * setpoint along z-direction, which is computed in PositionControl.cpp. */ - matrix::Vector3f sp{_sticks(0), _sticks(1), 0.0f}; - sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * sp); + Vector3f sp{_sticks(0), _sticks(1), 0.0f}; + sp = (Dcmf(Eulerf(0.0f, 0.0f, _yaw_sp)) * sp); if (sp.length() > 1.0f) { sp.normalize(); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index a5c9bc5176..0251cad327 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -61,7 +61,7 @@ void FlightTaskManualPosition::_scaleSticks() FlightTaskManualAltitude::_scaleSticks(); /* Constrain length of stick inputs to 1 for xy*/ - matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1)); + Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1)); float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f); @@ -70,13 +70,12 @@ void FlightTaskManualPosition::_scaleSticks() } /* Scale to velocity.*/ - matrix::Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get(); + Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get(); /* Rotate setpoint into local frame. */ - 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); + Vector3f vel_local = (Dcmf(Eulerf(0.0f, 0.0f, _yaw_sp)) * Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f)); + _vel_sp(0) = vel_local(0); + _vel_sp(1) = vel_local(1); } void FlightTaskManualPosition::_updateXYlock() diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp index 7ad3e8a18e..22ae8a5ad9 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp @@ -84,7 +84,8 @@ void FlightTaskManualStabilized::_updateThrustSetpoints() { /* Rotate setpoint into local frame. */ matrix::Vector3f sp{_sticks(0), _sticks(1), 0.0f}; - sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * sp); + + sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw_sp)) * sp); /* Ensure that maximum tilt is in [0.001, Pi] */ float tilt_max = math::constrain(math::radians(_tilt_max_man.get()), 0.001f, M_PI_F);