From 363ed43d38d2d7e235df10422a62d60de4930a70 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Fri, 30 Jun 2017 10:53:16 +0200
Subject: [PATCH] mc_pos_control: remove additional land-ground contact logic

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 68 -------------------
 1 file changed, 68 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index ab98d5138c..4f09ddb8c7 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -253,10 +253,6 @@ private:
 	math::Matrix<3, 3> _R;			/**< rotation matrix from attitude quaternions */
 	float _yaw;				/**< yaw angle (euler) */
 	float _yaw_takeoff;	/**< home yaw angle present when vehicle was taking off (euler) */
-	bool _in_landing;	/**< the vehicle is in the landing descent */
-	bool _lnd_reached_ground; /**< controller assumes the vehicle has reached the ground after landing */
-	float _vel_z_lp;
-	float _acc_z_lp;
 	float _vel_max_xy;  /**< equal to vel_max except in auto mode when close to target */
 
 	bool _in_takeoff = false; /**< flag for smooth velocity setpoint takeoff ramp */
@@ -426,10 +422,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_pos_first_nonfinite(true),
 	_yaw(0.0f),
 	_yaw_takeoff(0.0f),
-	_in_landing(false),
-	_lnd_reached_ground(false),
-	_vel_z_lp(0),
-	_acc_z_lp(0),
 	_vel_max_xy(0.0f),
 	_takeoff_vel_limit(0.0f),
 	_z_reset_counter(0),
@@ -1862,13 +1854,6 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 	float tilt_max = _params.tilt_max_air;
 	float thr_max = _params.thr_max;
 
-	/* filter vel_z over 1/8sec */
-	_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
-
-	/* filter vel_z change over 1/8sec */
-	float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
-	_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;
-
 	// We can only run the control if we're already in-air, have a takeoff setpoint,
 	// or if we're in offboard control.
 	// Otherwise, we should just bail out
@@ -1882,59 +1867,6 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 		/* adjust limits for landing mode */
 		/* limit max tilt and min lift when landing */
 		tilt_max = _params.tilt_max_land;
-
-		if (thr_min < 0.0f) {
-			thr_min = 0.0f;
-		}
-
-		/* descend stabilized, we're landing */
-		if (!_in_landing && !_lnd_reached_ground
-		    && (fabsf(_acc_z_lp) < 0.1f)
-		    && _vel_z_lp > 0.6f * _params.land_speed) {
-
-			_in_landing = true;
-		}
-
-		float land_z_threshold = 0.1f;
-
-		/* assume ground, cut thrust */
-		if (_in_landing
-		    && _vel_z_lp < land_z_threshold) {
-			thr_max = 0.0f;
-			_in_landing = false;
-			_lnd_reached_ground = true;
-
-		} else if (_in_landing
-			   && _vel_z_lp < math::min(0.3f * _params.land_speed, 2.5f * land_z_threshold)) {
-			/* not on ground but with ground contact, stop position and velocity control */
-			thrust_sp(0) = 0.0f;
-			thrust_sp(1) = 0.0f;
-			_vel_sp(0) = _vel(0);
-			_vel_sp(1) = _vel(1);
-			_pos_sp(0) = _pos(0);
-			_pos_sp(1) = _pos(1);
-		}
-
-		/* once we assumed to have reached the ground always cut the thrust.
-			Only free fall detection below can revoke this
-		*/
-		if (!_in_landing && _lnd_reached_ground) {
-			thr_max = 0.0f;
-		}
-
-		/* if we suddenly fall, reset landing logic and remove thrust limit */
-		if (_lnd_reached_ground
-		    /* XXX: magic value, assuming free fall above 4 m/s^2 acceleration */
-		    && (_acc_z_lp > 4.0f || _vel_z_lp > 2.0f * _params.land_speed)) {
-
-			thr_max = _params.thr_max;
-			_in_landing = true;
-			_lnd_reached_ground = false;
-		}
-
-	} else {
-		_in_landing = false;
-		_lnd_reached_ground = false;
 	}
 
 	/* limit min lift */