fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:58:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sticks: calculate expo only upon getter call

Browse Source
This commit is contained in:
Matthias Grob
2025-09-18 15:50:10 +02:00
committed by Silvan Fuhrer
parent 2e910fe185
commit 234e4688b0
2 changed files with 5 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sticks/Sticks.cpp
-8
View File
@@ -50,18 +50,11 @@ bool Sticks::checkAndUpdateStickInputs()
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// Linear scale
_positions(0) = manual_control_setpoint.pitch;
_positions(1) = manual_control_setpoint.roll;
_positions(2) = -manual_control_setpoint.throttle;
_positions(3) = manual_control_setpoint.yaw;
// Exponential scale
_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get());
_positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_man_deadzone.get());
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_man_deadzone.get());
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_man_deadzone.get());
_aux_positions(0) = manual_control_setpoint.aux1;
_aux_positions(1) = manual_control_setpoint.aux2;
_aux_positions(2) = manual_control_setpoint.aux3;
@@ -85,7 +78,6 @@ bool Sticks::checkAndUpdateStickInputs()
if (!_input_available) {
// Timeout: set all sticks to zero
_positions.zero();
_positions_expo.zero();
}
return _input_available;
src/lib/sticks/Sticks.hpp
+5 -10
View File
@@ -58,21 +58,17 @@ public:
bool isAvailable() { return _input_available; };
// Position : 0 : pitch, 1 : roll, 2 : throttle, 3 : yaw
const matrix::Vector4f &getPosition() { return _positions; }; // Raw stick position, no deadzone
const matrix::Vector4f &getPositionExpo() { return _positions_expo; }; // Deadzone and expo applied
// Helper functions to get stick values more intuitively
float getRoll() const { return _positions(1); }
float getRollExpo() const { return _positions_expo(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 _positions_expo(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 _positions_expo(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 -_positions_expo(2); }
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); }
const matrix::Vector2f getPitchRollExpo() { return _positions_expo.slice<2, 1>(0, 0); }
const matrix::Vector2f getPitchRollExpo() { return {getPitchExpo(), getRollExpo()}; }
const matrix::Vector<float, 6> &getAux() const { return _aux_positions; }
@@ -93,7 +89,6 @@ 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_expo; ///< modified manual sticks using expo function
matrix::Vector<float, 6> _aux_positions;