Slow landing improvement suggestion

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -175,7 +175,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		// Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate
 		float maxClimbRate = ((land_speed_threshold * 0.5f) < _params.maxClimbRate) ? (0.5f * land_speed_threshold) :
 				     _params.maxClimbRate;
-		verticalMovement = fabsf(_vehicleLocalPosition.z_deriv) > maxClimbRate;
+		verticalMovement = fabsf(_vehicleLocalPosition.vz) > maxClimbRate;
 	}
 
 	// Check if we are moving horizontally.

src/modules/mc_pos_control/PositionControl.cpp

@@ -55,24 +55,17 @@ void PositionControl::updateState(const PositionControlStates &states)
 	_vel_dot = states.acceleration;
 }
 
-void PositionControl::_setCtrlFlagTrue()
+void PositionControl::_setCtrlFlag(bool value)
 {
 	for (int i = 0; i <= 2; i++) {
-		_ctrl_pos[i] = _ctrl_vel[i] = true;
-	}
-}
-
-void PositionControl::_setCtrlFlagFalse()
-{
-	for (int i = 0; i <= 2; i++) {
-		_ctrl_pos[i] = _ctrl_vel[i] = false;
+		_ctrl_pos[i] = _ctrl_vel[i] = value;
 	}
 }
 
 bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
 {
-	// Only for logging purpose: by default we use the entire position-velocity control-loop pipeline
-	_setCtrlFlagTrue();
+	// by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose)
+	_setCtrlFlag(true);
 
 	_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
 	_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
@@ -212,8 +205,8 @@ bool PositionControl::_interfaceMapping()
 		// throttle down such that vehicle goes down with
 		// 70% of throttle range between min and hover
 		_thr_sp(2) = -(MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.7f);
-		// position and velocity control-loop is not used (note: only for logging purpose)
-		_setCtrlFlagFalse(); // position/velocity control-loop is not used
+		// position and velocity control-loop is currently unused (flag only for logging purpose)
+		_setCtrlFlag(false);
 	}
 
 	return !(failsafe);

src/modules/mc_pos_control/PositionControl.hpp

@@ -204,8 +204,7 @@ class PositionControl : public ModuleParams
 
 	void _positionController(); /** applies the P-position-controller */
 	void _velocityController(const float &dt); /** applies the PID-velocity-controller */
-	void _setCtrlFlagTrue(); /**< set control-loop flags to true (only required for logging) */
-	void _setCtrlFlagFalse(); /**< set control-loop flags to false (only required for logging) */
+	void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */
 
 	matrix::Vector3f _pos{}; /**< MC position */
 	matrix::Vector3f _vel{}; /**< MC velocity */

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1104,23 +1104,15 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float
 void
 MulticopterPositionControl::limit_thrust_during_landing(vehicle_local_position_setpoint_s &setpoint)
 {
-	if (_vehicle_land_detected.ground_contact) {
-		// Set thrust in xy to zero
-		setpoint.thrust[0] = 0.0f;
-		setpoint.thrust[1] = 0.0f;
-		_control.resetIntegralXY();
-		// set yaw-sp to current yaw
-		setpoint.yaw = _states.yaw;
-	}
-
-	if (_vehicle_land_detected.maybe_landed) {
-		// set yaw-sp to current yaw
-		setpoint.yaw = _states.yaw;
-		// we set thrust to zero
-		// this will help to decide if we are actually landed or not
+	if (_vehicle_land_detected.ground_contact
+	    || _vehicle_land_detected.maybe_landed) {
+		// we set thrust to zero, this will help to decide if we are actually landed or not
 		setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
-		_control.resetIntegralXY(); //reuqired to prevent integrator from increasing
-		_control.resetIntegralZ(); //reuqired to prevent integrator from increasing
+		// set yaw-sp to current yaw to avoid any corrections
+		setpoint.yaw = _states.yaw;
+		// prevent any integrator windup
+		_control.resetIntegralXY();
+		_control.resetIntegralZ();
 	}
 }