mc_pos_control: consider landing if not auto takeoff and valid

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1807,33 +1807,31 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 		thrust_sp(1) = 0.0f;
 	}
 
-	/* if still or already on ground command zero xy velcoity and zero xy thrust_sp in body frame to consider uneven ground */
-	if (_vehicle_land_detected.ground_contact && !in_auto_takeoff()) {
+	if (!in_auto_takeoff) {
+		if (_vehicle_land_detected.ground_contact) {
+			/* if still or already on ground command zero xy thrust_sp in body
+			 * frame to consider uneven ground */
 
+			/* thrust setpoint in body frame*/
+			math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
 
-		/* if still or already on ground command zero xy thrust_sp in body
-		 * frame to consider uneven ground */
+			/* we dont want to make any correction in body x and y*/
+			thrust_sp_body(0) = 0.0f;
+			thrust_sp_body(1) = 0.0f;
 
-		/* thrust setpoint in body frame*/
-		math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
+			/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
+			thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
 
-		/* we dont want to make any correction in body x and y*/
-		thrust_sp_body(0) = 0.0f;
-		thrust_sp_body(1) = 0.0f;
-
-		/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
-		thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
-
-		/* convert back to local frame (NED) */
-		thrust_sp = _R * thrust_sp_body;
-	}
+			/* convert back to local frame (NED) */
+			thrust_sp = _R * thrust_sp_body;
+		}
 
-	if (_vehicle_land_detected.maybe_landed
-	    && !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
-		/* we set thrust to zero
-		 * this will help to decide if we are actually landed or not
-		 */
-		thrust_sp.zero();
+		if (_vehicle_land_detected.maybe_landed) {
+			/* we set thrust to zero
+			 * this will help to decide if we are actually landed or not
+			 */
+			thrust_sp.zero();
+		}
 	}
 
 	if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) {