From 7ebd6cc74f87b91032588b550808cf3bed231cad Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 14 Apr 2023 15:46:41 +0200 Subject: [PATCH] FlightTaskManualPosition: use existing stick library rotation --- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 8 -------- .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp | 5 ----- .../tasks/ManualPosition/FlightTaskManualPosition.cpp | 4 ++-- 3 files changed, 2 insertions(+), 15 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 5e4f673dde..90d5dccb1f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -273,14 +273,6 @@ void FlightTaskManualAltitude::_respectGroundSlowdown() } } -void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v) -{ - const float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; - Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); - v(0) = v_r(0); - v(1) = v_r(1); -} - void FlightTaskManualAltitude::_updateHeadingSetpoints() { if (_isYawInput() || !_is_yaw_good_for_control) { diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 0ffe354735..0b9fbe72a3 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -63,11 +63,6 @@ protected: bool _checkTakeoff() override; void _updateConstraintsFromEstimator(); - /** - * rotates vector into local frame - */ - void _rotateIntoHeadingFrame(matrix::Vector2f &vec); - /** * Check and sets for position lock. * If sticks are at center position, the vehicle diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 24b98f35ef..71dedaf056 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -91,8 +91,8 @@ void FlightTaskManualPosition::_scaleSticks() // scale velocity to its maximum limits Vector2f vel_sp_xy = stick_xy * velocity_scale; - /* Rotate setpoint into local frame. */ - _rotateIntoHeadingFrame(vel_sp_xy); + // Rotate setpoint into local frame + Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint); // collision prevention if (_collision_prevention.is_active()) {