Multicopter land detector: Enforce sync between system and detector

src/modules/land_detector/LandDetector.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -68,7 +68,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
 	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
-	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
+	_paramHandle.maxThrottle = param_find("MPC_THR_MIN");
+	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
 	_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
 }
@@ -144,34 +145,45 @@ bool MulticopterLandDetector::get_freefall_state()
 
 bool MulticopterLandDetector::get_landed_state()
 {
+	// Time base for this function
+	const uint64_t now = hrt_absolute_time();
+
+	// Check if thrust output is less than max throttle param.
+	bool minimalThrust = (_actuators.control[3] <= (_params.maxThrottle + 0.05f));
+
+	if (minimalThrust && _min_trust_start == 0) {
+		_min_trust_start = now;
+	} else if (!minimalThrust) {
+		_min_trust_start = 0;
+	}
+
 	// 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();
+		_arming_time = now;
 	}
 
-	// Check if user commands throttle and if so, report not landed
-	if (_manual.z > 0.3f) {
-		return false;
-	}
-
-	// Check if thrust output is less than max throttle param.
-	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
-
 	// Return status based on armed state and throttle if no position lock is available.
 	if (_vehicleLocalPosition.timestamp == 0 ||
 	    hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
 	    !_vehicleLocalPosition.xy_valid ||
 	    !_vehicleLocalPosition.z_valid) {
 
-		// Minimal thrust means landed.
+	    	// Check if user commands throttle and if so, report not landed
-		return minimalThrust;
+		if (_manual.z > _params.minManThrottle + 0.01f) {
-	}
+			return false;
+		}
 
-	const uint64_t now = hrt_absolute_time();
+		if ((_min_trust_start > 0) &&
+			(hrt_elapsed_time(&_min_trust_start) > 5 * 1000 * 1000)) {
+			return !get_freefall_state();
+		} else {
+			return false;
+		}
+	}
 
 	float armThresholdFactor = 1.0f;
 
@@ -224,6 +236,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
 		_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
 		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
+		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
 		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
 	}

src/modules/land_detector/MulticopterLandDetector.h

@@ -102,6 +102,7 @@ private:
 		param_t maxVelocity;
 		param_t maxRotation;
 		param_t maxThrottle;
+		param_t minManThrottle;
 		param_t acc_threshold_m_s2;
 		param_t ff_trigger_time;
 	}		_paramHandle;
@@ -111,6 +112,7 @@ private:
 		float maxVelocity;
 		float maxRotation_rad_s;
 		float maxThrottle;
+		float minManThrottle;
 		float acc_threshold_m_s2;
 		float ff_trigger_time;
 	} _params;
@@ -129,10 +131,11 @@ private:
 	struct actuator_armed_s			_arming;
 	struct vehicle_attitude_s		_vehicleAttitude;
 	struct manual_control_setpoint_s	_manual;
-	struct control_state_s				_ctrl_state;
+	struct control_state_s			_ctrl_state;
 
-	uint64_t _landTimer;							/**< timestamp in microseconds since a possible land was detected*/
+	uint64_t _landTimer;			///< timestamp in microseconds since a possible land was detected
-	uint64_t _freefallTimer;							/**< timestamp in microseconds since a possible freefall was detected*/
+	uint64_t _freefallTimer;		///< timestamp in microseconds since a possible freefall was detected
+	uint64_t _min_trust_start;		///< timestamp when minimum trust was applied first
 };
 
 }

src/modules/land_detector/land_detector_params.c

@@ -74,20 +74,6 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.50f);
  */
 PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
 
-/**
- * Multicopter max throttle
- *
- * Maximum actuator output on throttle allowed in the landed state
- *
- * @unit norm
- * @min 0.1
- * @max 0.5
- * @decimal 2
- *
- * @group Land Detector
- */
-PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f);
-
 /**
  * Multicopter specific force threshold
  *
@@ -105,7 +91,7 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f);
 /**
  * Multicopter free-fall trigger time
  *
- * Milliseconds that freefall conditions have to hold before triggering a freefall.
+ * Seconds (decimal) that freefall conditions have to met before triggering a freefall.
  * Minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h
  *
  * @unit s