From 5b9455731014de491ca77f59ed6e25afbdf59656 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 4 Sep 2025 15:27:38 +0200 Subject: [PATCH] Sticks: change internal order and sign of stick positions --- src/lib/sticks/Sticks.cpp | 8 ++++---- src/lib/sticks/Sticks.hpp | 22 ++++++++++++---------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/lib/sticks/Sticks.cpp b/src/lib/sticks/Sticks.cpp index 31d06421f3..26a66cf271 100644 --- a/src/lib/sticks/Sticks.cpp +++ b/src/lib/sticks/Sticks.cpp @@ -50,10 +50,10 @@ bool Sticks::checkAndUpdateStickInputs() manual_control_setpoint_s manual_control_setpoint; if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _positions(0) = manual_control_setpoint.pitch; - _positions(1) = manual_control_setpoint.roll; - _positions(2) = -manual_control_setpoint.throttle; - _positions(3) = manual_control_setpoint.yaw; + _positions(0) = manual_control_setpoint.roll; + _positions(1) = manual_control_setpoint.pitch; + _positions(2) = manual_control_setpoint.yaw; + _positions(3) = manual_control_setpoint.throttle; _aux_positions(0) = manual_control_setpoint.aux1; _aux_positions(1) = manual_control_setpoint.aux2; diff --git a/src/lib/sticks/Sticks.hpp b/src/lib/sticks/Sticks.hpp index 56d6016386..1620fb90ce 100644 --- a/src/lib/sticks/Sticks.hpp +++ b/src/lib/sticks/Sticks.hpp @@ -59,15 +59,17 @@ public: bool isAvailable() { return _input_available; }; // Helper functions to get stick values more intuitively - float getRoll() const { return _positions(1); } - float getRollExpo() const { return math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); } - float getPitch() const { return _positions(0); } - float getPitchExpo() const { return math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); } - float getYaw() const { return _positions(3); } - float getYawExpo() const { return math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_man_deadzone.get()); } - float getThrottleZeroCentered() const { return -_positions(2); } // Convert Z-axis(down) command to Up-axis frame - float getThrottleZeroCenteredExpo() const { return -math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_man_deadzone.get()); } - const matrix::Vector2f getPitchRoll() { return _positions.slice<2, 1>(0, 0); } + float getRoll() const { return _positions(0); } + float getPitch() const { return _positions(1); } + float getYaw() const { return _positions(2); } + float getThrottleZeroCentered() const { return _positions(3); } + + float getRollExpo() const { return math::expo_deadzone(getRoll(), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); } + float getPitchExpo() const { return math::expo_deadzone(getPitch(), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); } + float getYawExpo() const { return math::expo_deadzone(getYaw(), _param_mpc_yaw_expo.get(), _param_man_deadzone.get()); } + float getThrottleZeroCenteredExpo() const { return math::expo_deadzone(getThrottleZeroCentered(), _param_mpc_z_man_expo.get(), _param_man_deadzone.get()); } + + const matrix::Vector2f getPitchRoll() { return {getPitch(), getRoll()}; } const matrix::Vector2f getPitchRollExpo() { return {getPitchExpo(), getRollExpo()}; } const matrix::Vector &getAux() const { return _aux_positions; } @@ -88,7 +90,7 @@ public: private: bool _input_available{false}; - matrix::Vector4f _positions; ///< unmodified manual stick inputs that usually move vehicle in x, y, z and yaw direction + matrix::Vector4f _positions; ///< unmodified manual stick inputs roll, pitch, yaw ,throttle matrix::Vector _aux_positions;