From 321440281b1cb7dce062ce1883046888be8daff9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 5 Apr 2016 15:32:13 -0700
Subject: [PATCH] Fix land detection for altitude hold

---
 src/modules/land_detector/MulticopterLandDetector.cpp | 8 ++++++++
 src/modules/land_detector/MulticopterLandDetector.h   | 3 +++
 2 files changed, 11 insertions(+)

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index 2c6a7bdafc..650ae5724d 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -53,6 +53,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_armingSub(-1),
 	_parameterSub(-1),
 	_attitudeSub(-1),
+	_manualSub(-1),
 	_vehicleLocalPosition{},
 	_actuators{},
 	_arming{},
@@ -73,6 +74,7 @@ void MulticopterLandDetector::initialize()
 	_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
+	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	// download parameters
 	updateParameterCache(true);
@@ -84,6 +86,7 @@ void MulticopterLandDetector::updateSubscriptions()
 	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
 	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
 	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
 }
 
 bool MulticopterLandDetector::update()
@@ -107,6 +110,11 @@ bool MulticopterLandDetector::get_landed_state()
 		_arming_time = hrt_absolute_time();
 	}
 
+	// 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;
 
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
index b5e308cc42..4e520fea94 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -49,6 +49,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <systemlib/param/param.h>
 
 class MulticopterLandDetector : public LandDetector
@@ -107,11 +108,13 @@ private:
 	int _armingSub;
 	int _parameterSub;
 	int _attitudeSub;
+	int _manualSub;
 
 	struct vehicle_local_position_s		_vehicleLocalPosition;
 	struct actuator_controls_s		_actuators;
 	struct actuator_armed_s			_arming;
 	struct vehicle_attitude_s		_vehicleAttitude;
+	struct manual_control_setpoint_s	_manual;
 
 	/* timestamp in microseconds since a possible land was detected */
 	uint64_t _landTimer;