fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 18:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sticks: change internal order and sign of stick positions

Browse Source
This commit is contained in:
Matthias Grob
2025-09-18 15:50:10 +02:00
committed by Silvan Fuhrer
parent 234e4688b0
commit 5b94557310
2 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sticks/Sticks.cpp
+4 -4
View File
@@ -50,10 +50,10 @@ bool Sticks::checkAndUpdateStickInputs()
manual_control_setpoint_s manual_control_setpoint; manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_positions(0) = manual_control_setpoint.pitch; _positions(0) = manual_control_setpoint.roll;
_positions(1) = manual_control_setpoint.roll; _positions(1) = manual_control_setpoint.pitch;
_positions(2) = -manual_control_setpoint.throttle; _positions(2) = manual_control_setpoint.yaw;
_positions(3) = manual_control_setpoint.yaw; _positions(3) = manual_control_setpoint.throttle;
_aux_positions(0) = manual_control_setpoint.aux1; _aux_positions(0) = manual_control_setpoint.aux1;
_aux_positions(1) = manual_control_setpoint.aux2; _aux_positions(1) = manual_control_setpoint.aux2;
src/lib/sticks/Sticks.hpp
+12 -10
View File
@@ -59,15 +59,17 @@ public:
bool isAvailable() { return _input_available; }; bool isAvailable() { return _input_available; };
// Helper functions to get stick values more intuitively // Helper functions to get stick values more intuitively
float getRoll() const { return _positions(1); } float getRoll() const { return _positions(0); }
float getRollExpo() const { return math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); } float getPitch() const { return _positions(1); }
float getPitch() const { return _positions(0); } float getYaw() const { return _positions(2); }
float getPitchExpo() const { return math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); } float getThrottleZeroCentered() const { return _positions(3); }
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 getRollExpo() const { return math::expo_deadzone(getRoll(), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); }
float getThrottleZeroCentered() const { return -_positions(2); } // Convert Z-axis(down) command to Up-axis frame float getPitchExpo() const { return math::expo_deadzone(getPitch(), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get()); }
float getThrottleZeroCenteredExpo() const { return -math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_man_deadzone.get()); } float getYawExpo() const { return math::expo_deadzone(getYaw(), _param_mpc_yaw_expo.get(), _param_man_deadzone.get()); }
const matrix::Vector2f getPitchRoll() { return _positions.slice<2, 1>(0, 0); } 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::Vector2f getPitchRollExpo() { return {getPitchExpo(), getRollExpo()}; }
const matrix::Vector<float, 6> &getAux() const { return _aux_positions; } const matrix::Vector<float, 6> &getAux() const { return _aux_positions; }
@@ -88,7 +90,7 @@ public:
private: private:
bool _input_available{false}; 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<float, 6> _aux_positions; matrix::Vector<float, 6> _aux_positions;