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

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

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<float, 4> _sticks; /**< unmodified manual stick inputs */
-	matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/
+	matrix::Vector<float, 4> _sticks_expo; /**< modified manual sticks using expo function*/
 
 	float stickDeadzone() const { return _stick_dz.get(); }
 
@@ -76,6 +76,8 @@ private:
 					(ParamFloat<px4::params::MPC_XY_MAN_EXPO>)
 					_xy_vel_man_expo, /**< ratio of exponential curve for stick input in xy direction */
 					(ParamFloat<px4::params::MPC_Z_MAN_EXPO>)
-					_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<px4::params::MPC_YAW_EXPO>)
+					_yaw_expo /**< ratio of exponential curve for stick input in yaw for modes except acro */
 				       )
 };

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp

@@ -37,6 +37,7 @@
 
 #include "FlightTaskManualStabilized.hpp"
 #include <mathlib/mathlib.h>
+#include <float.h>
 
 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;
+		}
 	}
 }
 

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)
  *