correct thrust limiting during landing with margin, don't reset position...

correct thrust limiting during landing with margin, don't reset position setpoint when switching from takeoff to posctl and allow high enough z velocity for position lock

ROMFS/px4fmu_common/init.d/4030_solo

@@ -24,6 +24,7 @@ then
     param set MIS_TAKEOFF_ALT   5.0
     param set COM_DISARM_LAND   1
     param set RTL_LAND_DELAY    1
+    param set MPC_HOLD_MAX_Z    1.5
 
     # enable high bandwidth mavlink by default
     param set SYS_COMPANION     921601

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -242,6 +242,7 @@ private:
 	math::Matrix<3, 3> _R;			/**< rotation matrix from attitude quaternions */
 	float _yaw;				/**< yaw angle (euler) */
 	float _landing_thrust;
+	hrt_abstime _landing_started;
 
 	/**
 	 * Update our local parameter cache.
@@ -372,7 +373,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_run_pos_control(true),
 	_run_alt_control(true),
 	_yaw(0.0f),
-	_landing_thrust(1.0f)
+	_landing_thrust(1.0f),
+	_landing_started(0)
 {
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
 	memset(&_ctrl_state, 0, sizeof(_ctrl_state));
@@ -951,10 +953,6 @@ void MulticopterPositionControl::control_auto(float dt)
 	}
 
 	if (current_setpoint_valid) {
-		/* in case of interrupted mission don't go to waypoint but stay at current position */
-		_reset_pos_sp = true;
-		_reset_alt_sp = true;
-
 		/* scaled space: 1 == position error resulting max allowed speed */
 		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
 
@@ -1059,14 +1057,20 @@ void MulticopterPositionControl::control_auto(float dt)
 			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
 		}
 
-		/* if we're near the current pos SP don't reset it anymore, else it will make if jump back,
-		 * especially noticable in altitude after takeoff.
+		/* 
+		 * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
+		 * this makes the takeoff finish smoothly.
 		 */
-		if (current_setpoint_valid
+		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
 				&& _pos_sp_triplet.current.acceptance_radius > 0.0f
-				&& (curr_sp - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius) {
+				&& (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.1f) {
 			_reset_pos_sp = false;
 			_reset_alt_sp = false;
+
+		/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
+		} else {
+			_reset_pos_sp = true;
+			_reset_alt_sp = true;
 		}
 
 	} else {
@@ -1432,6 +1436,7 @@ MulticopterPositionControl::task_main()
 						thr_min = 0.0f;
 					}
 
+					float thrust_abs = thrust_sp.length();
 					float tilt_max = _params.tilt_max_air;
 					float thr_max = _params.thr_max;
 
@@ -1440,19 +1445,26 @@ MulticopterPositionControl::task_main()
 					    _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 						/* limit max tilt and min lift when landing */
 						tilt_max = _params.tilt_max_land;
+						if (_landing_started == 0) {
+							_landing_started = hrt_absolute_time();
+						}
 
 						if (thr_min < 0.0f) {
 							thr_min = 0.0f;
 						}
 
 						/* don't let it throttle up again during landing */
-						if (thrust_sp(2) < 0.0f && thrust_sp(2) < _landing_thrust) {
-							_landing_thrust = thrust_sp(2);
+						if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust
+								&& hrt_elapsed_time(&_landing_started) > 15e5) {
+							_landing_thrust = thrust_abs;
 						}
-						// XXX: we probably need to add a margin here becaue we're limiting the complete thrust vector further down
-						thr_max = -_landing_thrust;
+						/* add 5% to give it some margin to compensate external influences */
+						thr_max = _landing_thrust + 0.05f * _landing_thrust;
+						/*PX4_WARN("thrust: abs %.4f, sp(2) %.4f, _landing_thrust %.4f, vel_err(2) %.4f, vel_sp(2) %.4f, vel(2) %.4f",
+							(double) thrust_abs, (double) thrust_sp(2), (double) _landing_thrust, (double) vel_err(2), (double) _vel_sp(2), (double) _vel(2));*/
 
 					} else {
+						_landing_started = 0;
 						_landing_thrust = _params.thr_max;
 					}
 
@@ -1503,8 +1515,6 @@ MulticopterPositionControl::task_main()
 					}
 
 					/* limit max thrust */
-					float thrust_abs = thrust_sp.length();
-
 					if (thrust_abs > thr_max) {
 						if (thrust_sp(2) < 0.0f) {
 							if (-thrust_sp(2) > thr_max) {