dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Sticks: Provide auxiliary analog values from manual_control_setpoint

This commit is contained in:
Matthias Grob 2023-08-14 15:00:33 +02:00
parent df41bc3d26
commit 77c06a9f9e
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
View File

@ -62,6 +62,13 @@ bool Sticks::checkAndUpdateStickInputs()
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
_aux_positions(0) = manual_control_setpoint.aux1;
_aux_positions(1) = manual_control_setpoint.aux2;
_aux_positions(2) = manual_control_setpoint.aux3;
_aux_positions(3) = manual_control_setpoint.aux4;
_aux_positions(4) = manual_control_setpoint.aux5;
_aux_positions(5) = manual_control_setpoint.aux6;
// valid stick inputs are required
_input_available = manual_control_setpoint.valid && _positions.isAllFinite();

4
src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp
View File

@ -74,6 +74,8 @@ public:
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::Vector<float, 6> &getAux() const { return _aux_positions; }
/**
* Limit the the horizontal input from a square shaped joystick gimbal to a unit circle
* @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted
@ -93,6 +95,8 @@ private:
matrix::Vector4f _positions; ///< unmodified manual stick inputs
matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function
matrix::Vector<float, 6> _aux_positions;
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)};