add parameter for hover throttle setting

and tighten z deadband

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -121,8 +121,8 @@ public:
 	int		start();
 
 private:
-    const float alt_ctl_dz = 0.1f;
-    const float alt_ctl_dy = 0.05f;
+    const float alt_ctl_dz = 0.05f;
+    const float alt_ctl_dy = 0.025f;
 
 	bool		_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
@@ -167,6 +167,7 @@ private:
 	struct {
 		param_t thr_min;
 		param_t thr_max;
+		param_t thr_hover;
 		param_t z_p;
 		param_t z_vel_p;
 		param_t z_vel_i;
@@ -199,6 +200,7 @@ private:
 	struct {
 		float thr_min;
 		float thr_max;
+		float thr_hover;
 		float tilt_max_air;
 		float land_speed;
 		float tko_speed;
@@ -269,7 +271,8 @@ private:
 	 */
 	void		poll_subscriptions();
 
-    static float	scale_control(float ctl, float end, float dz, float dy);
+	static float	scale_control(float ctl, float end, float dz, float dy);
+	static float    throttle_curve(float ctl, float ctr);
 
 	/**
 	 * Update reference for local position projection
@@ -419,6 +422,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
+	_params_handles.thr_hover	= param_find("MPC_THR_HOVER");
 	_params_handles.z_p			= param_find("MPC_Z_P");
 	_params_handles.z_vel_p		= param_find("MPC_Z_VEL_P");
 	_params_handles.z_vel_i		= param_find("MPC_Z_VEL_I");
@@ -493,6 +497,7 @@ MulticopterPositionControl::parameters_update(bool force)
 		/* update legacy C interface params */
 		param_get(_params_handles.thr_min, &_params.thr_min);
 		param_get(_params_handles.thr_max, &_params.thr_max);
+		param_get(_params_handles.thr_hover, &_params.thr_hover);
 		param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
 		_params.tilt_max_air = math::radians(_params.tilt_max_air);
 		param_get(_params_handles.land_speed, &_params.land_speed);
@@ -639,13 +644,26 @@ float
 MulticopterPositionControl::scale_control(float ctl, float end, float dz, float dy)
 {
 	if (ctl > dz) {
-        return dy + (ctl - dz) * (1.0f - dy) / (end - dz);
+		return dy + (ctl - dz) * (1.0f - dy) / (end - dz);
 
 	} else if (ctl < -dz) {
-        return -dy + (ctl + dz) * (1.0f - dy) / (end - dz);
+		return -dy + (ctl + dz) * (1.0f - dy) / (end - dz);
 
 	} else {
-        return ctl * (dy / dz);
+		return ctl * (dy / dz);
+	}
+}
+
+float
+MulticopterPositionControl::throttle_curve(float ctl, float ctr)
+{
+	/* piecewise linear mapping: 0:ctr -> 0:0.5
+	 * and ctr:1 -> 0.5:1 */
+	if (ctl < 0.5f) {
+		return 2 * ctl * ctr;
+
+	} else {
+		return ctr + 2 * (ctl - 0.5f) * (1.0f - ctr);
 	}
 }
 
@@ -738,8 +756,8 @@ MulticopterPositionControl::control_manual(float dt)
 
 	if (_control_mode.flag_control_altitude_enabled) {
 		/* set vertical velocity setpoint with throttle stick */
-        req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz, alt_ctl_dy); // D
-    }
+		req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz, alt_ctl_dy); // D
+	}
 
 	if (_control_mode.flag_control_position_enabled) {
 		/* set horizontal velocity setpoint with roll/pitch stick */
@@ -1462,7 +1480,7 @@ MulticopterPositionControl::task_main()
 							float i = _params.thr_min;
 
 							if (reset_int_z_manual) {
-								i = _manual.z;
+								i = _params.thr_hover;
 
 								if (i < _params.thr_min) {
 									i = _params.thr_min;
@@ -1840,7 +1858,8 @@ MulticopterPositionControl::task_main()
 
 			/* control throttle directly if no climb rate controller is active */
 			if (!_control_mode.flag_control_climb_rate_enabled) {
-				_att_sp.thrust = math::min(_manual.z, _manual_thr_max.get());
+				float thr_val = throttle_curve(_manual.z, _params.thr_hover);
+				_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());
 
 				/* enforce minimum throttle if not landed */
 				if (!_vehicle_status.condition_landed) {

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -50,6 +50,17 @@
  */
 PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
 
+/**
+ * Hover thrust
+ *
+ * Vertical thrust required to hover.
+ *
+ * @min 0.2
+ * @max 0.8
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f);
+
 /**
  * Maximum thrust in auto thrust control
  *