From 70802ba15aa521fd2b9e82053cac4df306a836dc Mon Sep 17 00:00:00 2001 From: MaEtUgR Date: Wed, 20 Jun 2018 19:41:04 +0200 Subject: [PATCH] FlightTaskManual: Yaw lock with deadzone and expo More than a year ago I started the easy to use math::Functions to handle the always used mathematical SISO functions to be tested and available. I switched x, y and z stick input to the freesh programmed deadzone and exponential functions from the library to unify and clarify their use. I just realized yaw was left over because it lead to a drift problem in certain new use cases. Now I'm just adding the yaw stick to the already well working method. --- src/lib/FlightTasks/tasks/FlightTaskManual.cpp | 1 + src/lib/FlightTasks/tasks/FlightTaskManual.hpp | 6 ++++-- .../tasks/FlightTaskManualStabilized.cpp | 17 ++++++++++------- .../mc_pos_control/mc_pos_control_params.c | 16 ++++++++++++++++ 4 files changed, 31 insertions(+), 9 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 230b8392e0..22039dddd0 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -80,6 +80,7 @@ bool FlightTaskManual::_evaluateSticks() _sticks_expo(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _stick_dz.get()); _sticks_expo(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _stick_dz.get()); _sticks_expo(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _stick_dz.get()); + _sticks_expo(3) = math::expo_deadzone(_sticks(3), _yaw_expo.get(), _stick_dz.get()); // Only switch the landing gear up if the user switched from gear down to gear up. // If the user had the switch in the gear up position and took off ignore it diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 411646cad0..da48801dfa 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -60,7 +60,7 @@ protected: bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */ matrix::Vector _sticks; /**< unmodified manual stick inputs */ - matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/ + matrix::Vector _sticks_expo; /**< modified manual sticks using expo function*/ float stickDeadzone() const { return _stick_dz.get(); } @@ -76,6 +76,8 @@ private: (ParamFloat) _xy_vel_man_expo, /**< ratio of exponential curve for stick input in xy direction */ (ParamFloat) - _z_vel_man_expo /**< ratio of exponential curve for stick input in z direction */ + _z_vel_man_expo, /**< ratio of exponential curve for stick input in z direction */ + (ParamFloat) + _yaw_expo /**< ratio of exponential curve for stick input in yaw for modes except acro */ ) }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp index 8c0a1c9f0a..47c9faa8ee 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp @@ -37,6 +37,7 @@ #include "FlightTaskManualStabilized.hpp" #include +#include using namespace matrix; @@ -52,7 +53,7 @@ void FlightTaskManualStabilized::_scaleSticks() { /* Scale sticks to yaw and thrust using * linear scale for yaw and piecewise linear map for thrust. */ - _yawspeed_setpoint = _sticks(3) * math::radians(_yaw_rate_scaling.get()); + _yawspeed_setpoint = _sticks_expo(3) * math::radians(_yaw_rate_scaling.get()); _throttle = _throttleCurve(); } @@ -61,13 +62,15 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints() /* Yaw-lock depends on stick input. If not locked, * yaw_sp is set to NAN. * TODO: add yawspeed to get threshold.*/ - const bool stick_yaw_zero = fabsf(_sticks(3)) <= stickDeadzone(); - - if (stick_yaw_zero && !PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint = _yaw; - - } else if (!stick_yaw_zero) { + if (_yawspeed_setpoint > FLT_EPSILON) { + // no fixed heading when rotating around yaw by stick _yaw_setpoint = NAN; + + } else { + // hold the current heading when no more rotation commanded + if (!PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint = _yaw; + } } } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index df68d1f7ba..3bf3c8265d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -550,6 +550,22 @@ PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.0f); */ PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.0f); +/** + * Manual control stick yaw rotation exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve (default) + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_YAW_EXPO, 0.0f); + /** * Altitude for 1. step of slow landing (descend) *