diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index 575da6e353..fb5bc40673 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -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(); diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp index 4b49134d19..be8b69e306 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -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 &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 _aux_positions; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)};