Make land detector more robust during initial spool-up

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_parameterSub(-1),
-    _attitudeSub(-1),
-	_vehicleGlobalPosition({}),
-	_vehicleStatus({}),
-	_actuators({}),
-	_arming({}),
-    _vehicleAttitude({}),
+	_attitudeSub(-1),
+	_vehicleGlobalPosition{},
+	_vehicleStatus{},
+	_actuators{},
+	_arming{},
+	_vehicleAttitude{},
 	_landTimer(0)
 {
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
@@ -97,7 +97,10 @@ bool MulticopterLandDetector::update()
 
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
+		_arming_time = 0;
 		return true;
+	} else if (_arming_time == 0) {
+		_arming_time = hrt_absolute_time();
 	}
 
 	// return status based on armed state if no position lock is available
@@ -110,8 +113,18 @@ bool MulticopterLandDetector::update()
 
 	const uint64_t now = hrt_absolute_time();
 
-	// check if we are moving vertically
-	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
+	float armThresholdFactor = 1.0f;
+
+	// Widen acceptance thresholds for landed state right after arming
+	// so that motor spool-up and other effects do not trigger false negatives
+	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
+		armThresholdFactor = 2.5f;
+	}
+
+	// check if we are moving vertically - this might see a spike after arming due to
+	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
+	// an accurate in-air indication
+	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor;
 
 	// check if we are moving horizontally
 	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
@@ -119,9 +132,11 @@ bool MulticopterLandDetector::update()
 					&& _vehicleStatus.condition_global_position_valid;
 
 	// next look if all rotation angles are not moving
-	bool rotating = fabsf(_vehicleAttitude.rollspeed)  > _params.maxRotation ||
-				    fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation ||
-        			fabsf(_vehicleAttitude.yawspeed)   > _params.maxRotation;
+	float maxRotationScaled = _params.maxRotation * armThresholdFactor;
+
+	bool rotating = (fabsf(_vehicleAttitude.rollspeed)  > maxRotationScaled) ||
+				    (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
+				(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
 
 	// check if thrust output is minimal (about half of default)
 	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;