mc_pos_control: moved responsibility of the x,y stick deadzone to a m…

…athematical function

to prevent a setpoint step when moving the stick over the border of the deadzone
and to enable very small inputs even from a control stick that needs a big deadzone

src/lib/mathlib/math/Expo.hpp

@@ -35,7 +35,7 @@
  * @file Expo.hpp
  *
  * So called exponential curve function implementation.
- * It's essentially a linear combination between a linear and a cubic function.
+ * It is essentially a linear combination between a linear and a cubic function.
  */
 
 #pragma once
@@ -46,11 +46,33 @@
 namespace math
 {
 
+// Type-safe signum function
+template<typename T> int sign(T val) {
+	return (T(0) < val) - (val < T(0));
+}
+
 template<typename _Tp>
 inline const _Tp expo(const _Tp &value, const _Tp &e)
 {
 	_Tp x = constrain(value ,(_Tp)-1, (_Tp)1);
 	return (1-e)*x + e*x*x*x;
 }
 
+template<typename _Tp>
+inline const _Tp deadzone(const _Tp &value, const _Tp &dz)
+{
+	_Tp x = constrain(value ,(_Tp)-1, (_Tp)1);
+	// Rescale the input such that we get a piecewise linear function that will be continuous with applied deadzone
+	_Tp out = (x-sign(x)*dz)/(1-dz);
+	// apply the deadzone (values zero around the middle)
+	return out * (fabsf(x) > dz);
+}
+
+template<typename _Tp>
+inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz)
+{
+	_Tp x = constrain(value ,(_Tp)-1, (_Tp)1);
+	return expo(deadzone(x, dz),e);
+}
+
 }

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -939,8 +939,8 @@ MulticopterPositionControl::control_manual(float dt)
 
 	if (_control_mode.flag_control_position_enabled) {
 		/* set horizontal velocity setpoint with roll/pitch stick */
-		req_vel_sp_xy(0) = math::expo(_manual.x, _params.xy_vel_man_expo);
-		req_vel_sp_xy(1) = math::expo(_manual.y, _params.xy_vel_man_expo);
+		req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_xy_dz);
+		req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_xy_dz);
 
 		/* reset position setpoint to current position if needed */
 		reset_pos_sp();
@@ -1006,15 +1006,15 @@ MulticopterPositionControl::control_manual(float dt)
 	if (_pos_hold_engaged) {
 
 		/* only switch back to velocity control if user moves stick */
-		_pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < _params.hold_xy_dz)
-				    && (fabsf(req_vel_sp_xy(1)) < _params.hold_xy_dz);
+		_pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON)
+				    && (fabsf(req_vel_sp_xy(1)) < FLT_EPSILON);
 
 	} else {
 
 		/* check if we switch to pos_hold_engaged */
 		float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));
 		bool smooth_pos_transition = _control_mode.flag_control_position_enabled &&
-					     (fabsf(req_vel_sp_xy(0)) < _params.hold_xy_dz && fabsf(req_vel_sp_xy(1)) < _params.hold_xy_dz) &&
+					     (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON && fabsf(req_vel_sp_xy(1)) < FLT_EPSILON) &&
 					     (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy);
 
 		/* during transition predict setpoint forward */