From 01073d36e57c089ab659fde2c77a26f81edcc7fb Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 15 Feb 2018 09:34:02 +0100
Subject: [PATCH] FlightTaskManualPosition: use axis angle to rotate into world
frame
---
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp | 7 ++++---
1 file changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
index 480368930b..1b755b21b8 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
@@ -74,9 +74,9 @@ void FlightTaskManualPosition::_scaleSticks()
_vel_sp_xy = stick_xy * _vel_xy_manual_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_xy = matrix::Vector2f(vel_world(0), vel_world(1));
}
void FlightTaskManualPosition::_updateXYlock()
@@ -90,6 +90,7 @@ void FlightTaskManualPosition::_updateXYlock()
_pos_sp_xy = matrix::Vector2f(&_position(0));
} else if (!apply_brake) {
+ /* Don't use position setpoint */
_pos_sp_xy = _pos_sp_xy * NAN;
}
}
--
GitLab