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

Sticks: only use stick input if flagged valid 

and add a function for just pitch roll stick input
This commit is contained in:
Matthias Grob 2023-02-24 10:19:08 +01:00
parent 1dada5daf4
commit da4644c20a
2 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;

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

@ -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<float, 4> &getPosition() { return _positions; };
const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
const matrix::Vector<float, 4> &getPosition() { return _positions; }; // Raw stick position, no deadzone
const matrix::Vector<float, 4> &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