diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index af2d28b41b..19388bc4ad 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,7 +63,7 @@ bool Sticks::checkAndUpdateStickInputs() _positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get()); // valid stick inputs are required - _input_available = _positions.isAllFinite(); + _input_available = manual_control_setpoint.valid && _positions.isAllFinite(); } else { failsafe_flags_s failsafe_flags; diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp index a43fbfef1d..fd7c3d1d01 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -59,18 +59,20 @@ public: bool isAvailable() { return _input_available; }; // Position : 0 : pitch, 1 : roll, 2 : throttle, 3 : yaw - const matrix::Vector &getPosition() { return _positions; }; - const matrix::Vector &getPositionExpo() { return _positions_expo; }; + const matrix::Vector &getPosition() { return _positions; }; // Raw stick position, no deadzone + const matrix::Vector &getPositionExpo() { return _positions_expo; }; // Deadzone and expo applied // Helper functions to get stick values more intuitively - float getPitch() const { return _positions(0); } float getRoll() const { return _positions(1); } - float getThrottleZeroCentered() const { return -_positions(2); } // Convert Z-axis(down) command to Up-axis frame - float getYaw() const { return _positions(3); } - float getPitchExpo() const { return _positions_expo(0); } float getRollExpo() const { return _positions_expo(1); } - float getThrottleZeroCenteredExpo() const { return -_positions_expo(2); } // Convert Z-axis(down) command to Up-axis frame + float getPitch() const { return _positions(0); } + float getPitchExpo() const { return _positions_expo(0); } + float getYaw() const { return _positions(3); } float getYawExpo() const { return _positions_expo(3); } + float getThrottleZeroCentered() const { return -_positions(2); } // Convert Z-axis(down) command to Up-axis frame + float getThrottleZeroCenteredExpo() const { return -_positions_expo(2); } + const matrix::Vector2f getPitchRoll() { return _positions.slice<2, 1>(0, 0); } + const matrix::Vector2f getPitchRollExpo() { return _positions_expo.slice<2, 1>(0, 0); } /** * Limit the the horizontal input from a square shaped joystick gimbal to a unit circle