Merge branch 'land_detector'

19 files changed, 1155 insertions, 67 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index aac6b2002..3c71195fb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -84,6 +84,7 @@
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vtol_vehicle_status.h>
+ #include <uORB/topics/vehicle_land_detected.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -1017,6 +1018,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct vehicle_local_position_s local_position;
 	memset(&local_position, 0, sizeof(local_position));
 
+	/* Subscribe to land detector */
+	int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	struct vehicle_land_detected_s land_detector;
+	memset(&land_detector, 0, sizeof(land_detector));
+
 	/*
 	 * The home position is set based on GPS only, to prevent a dependency between
 	 * position estimator and commander. RAW GPS is more than good enough for a
@@ -1405,9 +1411,15 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
 			    &(status.condition_local_altitude_valid), &status_changed);
 
+		/* Update land detector */
+		orb_check(land_detector_sub, &updated);
+		if(updated) {
+			orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
+		}
+
 		if (status.condition_local_altitude_valid) {
-			if (status.condition_landed != local_position.landed) {
-				status.condition_landed = local_position.landed;
+			if (status.condition_landed != land_detector.landed) {
+				status.condition_landed = land_detector.landed;
 				status_changed = true;
 
 				if (status.condition_landed) {
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index c41777968..866e5dc84 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -290,10 +290,6 @@ private:
 
 	AttPosEKF					*_ekf;
 
-	float						_velocity_xy_filtered;
-	float						_velocity_z_filtered;
-	float						_airspeed_filtered;
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -422,10 +418,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_mavlink_fd(-1),
 	_parameters{},
 	_parameter_handles{},
-	_ekf(nullptr),
-	_velocity_xy_filtered(0.0f),
-	_velocity_z_filtered(0.0f),
-	_airspeed_filtered(0.0f)
+	_ekf(nullptr)
 {
 
 	_last_run = hrt_absolute_time();
@@ -1434,22 +1427,6 @@ FixedwingEstimator::task_main()
 						_local_pos.v_z_valid = true;
 						_local_pos.xy_global = true;
 
-						_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
-						_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
-						_airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
-
-
-						/* crude land detector for fixedwing only,
-						* TODO: adapt so that it works for both, maybe move to another location
-						*/
-						if (_velocity_xy_filtered < 5
-							&& _velocity_z_filtered < 10
-							&& _airspeed_filtered < 10) {
-							_local_pos.landed = true;
-						} else {
-							_local_pos.landed = false;
-						}
-
 						_local_pos.z_global = false;
 						_local_pos.yaw = _att.yaw;
 
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
new file mode 100644
index 000000000..8e5bcf7ba
--- /dev/null
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FixedwingLandDetector.cpp
+ * Land detection algorithm for fixedwings
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#include "FixedwingLandDetector.h"
+
+#include <cmath>
+#include <drivers/drv_hrt.h>
+
+FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
+	_paramHandle(),
+	_params(),
+	_vehicleLocalPositionSub(-1),
+	_vehicleLocalPosition({}),
+	_airspeedSub(-1),
+	_airspeed({}),
+	_parameterSub(-1),
+	_velocity_xy_filtered(0.0f),
+	_velocity_z_filtered(0.0f),
+	_airspeed_filtered(0.0f),
+	_landDetectTrigger(0)
+{
+	_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
+	_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
+	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
+}
+
+void FixedwingLandDetector::initialize()
+{
+	// Subscribe to local position and airspeed data
+	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+}
+
+void FixedwingLandDetector::updateSubscriptions()
+{
+	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
+	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+}
+
+bool FixedwingLandDetector::update()
+{
+	// First poll for new data from our subscriptions
+	updateSubscriptions();
+
+	const uint64_t now = hrt_absolute_time();
+	bool landDetected = false;
+
+	// TODO: reset filtered values on arming?
+	_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
+				_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
+	_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+	_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+
+	// crude land detector for fixedwing
+	if (_velocity_xy_filtered < _params.maxVelocity
+	    && _velocity_z_filtered < _params.maxClimbRate
+	    && _airspeed_filtered < _params.maxAirSpeed) {
+
+		// these conditions need to be stable for a period of time before we trust them
+		if (now > _landDetectTrigger) {
+			landDetected = true;
+		}
+
+	} else {
+		// reset land detect trigger
+		_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
+	}
+
+	return landDetected;
+}
+
+void FixedwingLandDetector::updateParameterCache(const bool force)
+{
+	bool updated;
+	parameter_update_s paramUpdate;
+
+	orb_check(_parameterSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+	}
+
+	if (updated || force) {
+		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
+	}
+}
diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h
new file mode 100644
index 000000000..0e9c092ee
--- /dev/null
+++ b/src/modules/land_detector/FixedwingLandDetector.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FixedwingLandDetector.h
+ * Land detection algorithm for fixedwing
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#ifndef __FIXED_WING_LAND_DETECTOR_H__
+#define __FIXED_WING_LAND_DETECTOR_H__
+
+#include "LandDetector.h"
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+
+class FixedwingLandDetector : public LandDetector
+{
+public:
+	FixedwingLandDetector();
+
+protected:
+	/**
+	* @brief  blocking loop, should be run in a separate thread or task. Runs at 50Hz
+	**/
+	bool update() override;
+
+	/**
+	* @brief Initializes the land detection algorithm
+	**/
+	void initialize() override;
+
+	/**
+	* @brief  polls all subscriptions and pulls any data that has changed
+	**/
+	void updateSubscriptions();
+
+private:
+	/**
+	* @brief download and update local parameter cache
+	**/
+	void updateParameterCache(const bool force);
+
+	/**
+	* @brief Handles for interesting parameters
+	**/
+	struct {
+		param_t maxVelocity;
+		param_t maxClimbRate;
+		param_t maxAirSpeed;
+	}		_paramHandle;
+
+	struct {
+		float maxVelocity;
+		float maxClimbRate;
+		float maxAirSpeed;
+	} _params;
+
+private:
+	int                                     _vehicleLocalPositionSub;   /**< notification of local position */
+	struct vehicle_local_position_s         _vehicleLocalPosition;      /**< the result from local position subscription */
+	int                                     _airspeedSub;
+	struct airspeed_s                       _airspeed;
+	int 									_parameterSub;
+
+	float _velocity_xy_filtered;
+	float _velocity_z_filtered;
+	float _airspeed_filtered;
+	uint64_t _landDetectTrigger;
+};
+
+#endif //__FIXED_WING_LAND_DETECTOR_H__
diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
new file mode 100644
index 000000000..a4fbb9861
--- /dev/null
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file LandDetector.cpp
+ * Land detection algorithm
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ */
+
+#include "LandDetector.h"
+#include <unistd.h>                 //usleep
+#include <drivers/drv_hrt.h>
+
+LandDetector::LandDetector() :
+	_landDetectedPub(-1),
+	_landDetected({0, false}),
+	_taskShouldExit(false),
+	_taskIsRunning(false)
+{
+	// ctor
+}
+
+LandDetector::~LandDetector()
+{
+	_taskShouldExit = true;
+	close(_landDetectedPub);
+}
+
+void LandDetector::shutdown()
+{
+	_taskShouldExit = true;
+}
+
+void LandDetector::start()
+{
+	// make sure this method has not already been called by another thread
+	if (isRunning()) {
+		return;
+	}
+
+	// advertise the first land detected uORB
+	_landDetected.timestamp = hrt_absolute_time();
+	_landDetected.landed = false;
+	_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
+
+	// initialize land detection algorithm
+	initialize();
+
+	// task is now running, keep doing so until shutdown() has been called
+	_taskIsRunning = true;
+	_taskShouldExit = false;
+
+	while (isRunning()) {
+
+		bool landDetected = update();
+
+		// publish if land detection state has changed
+		if (_landDetected.landed != landDetected) {
+			_landDetected.timestamp = hrt_absolute_time();
+			_landDetected.landed = landDetected;
+
+			// publish the land detected broadcast
+			orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected);
+		}
+
+		// limit loop rate
+		usleep(1000000 / LAND_DETECTOR_UPDATE_RATE);
+	}
+
+	_taskIsRunning = false;
+	_exit(0);
+}
+
+bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
+{
+	bool newData = false;
+
+	// check if there is new data to grab
+	if (orb_check(handle, &newData) != OK) {
+		return false;
+	}
+
+	if (!newData) {
+		return false;
+	}
+
+	if (orb_copy(meta, handle, buffer) != OK) {
+		return false;
+	}
+
+	return true;
+}
diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h
new file mode 100644
index 000000000..09db6e474
--- /dev/null
+++ b/src/modules/land_detector/LandDetector.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file LandDetector.h
+ * Abstract interface for land detector algorithms
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#ifndef __LAND_DETECTOR_H__
+#define __LAND_DETECTOR_H__
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_land_detected.h>
+
+class LandDetector
+{
+public:
+
+	LandDetector();
+	virtual ~LandDetector();
+
+	/**
+	* @return true if this task is currently running
+	**/
+	inline bool isRunning() const {return _taskIsRunning;}
+
+	/**
+	* @brief  Tells the Land Detector task that it should exit
+	**/
+	void shutdown();
+
+	/**
+	* @brief Blocking function that should be called from it's own task thread. This method will
+	*        run the underlying algorithm at the desired update rate and publish if the landing state changes.
+	**/
+	void start();
+
+protected:
+
+	/**
+	* @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm
+	* @return true if a landing was detected and this should be broadcast to the rest of the system
+	**/
+	virtual bool update() = 0;
+
+	/**
+	* @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation,
+	*        uORB topic subscription, creating file descriptors, etc.)
+	**/
+	virtual void initialize() = 0;
+
+	/**
+	* @brief Convinience function for polling uORB subscriptions
+	* @return true if there was new data and it was successfully copied
+	**/
+	bool orb_update(const struct orb_metadata *meta, int handle, void *buffer);
+
+	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50;        /**< Run algorithm at 50Hz */
+
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
+                                                                          before triggering a land */
+
+protected:
+	orb_advert_t                            _landDetectedPub;           /**< publisher for position in local frame */
+	struct vehicle_land_detected_s          _landDetected;              /**< local vehicle position */
+
+private:
+	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */
+	bool _taskIsRunning;                                                /**< task has reached main loop and is currently running */
+};
+
+#endif //__LAND_DETECTOR_H__
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
new file mode 100644
index 000000000..277cb9363
--- /dev/null
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -0,0 +1,145 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file MulticopterLandDetector.cpp
+ * Land detection algorithm for multicopters
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ */
+
+#include "MulticopterLandDetector.h"
+
+#include <cmath>
+#include <drivers/drv_hrt.h>
+
+MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
+	_paramHandle(),
+	_params(),
+	_vehicleGlobalPositionSub(-1),
+	_sensorsCombinedSub(-1),
+	_waypointSub(-1),
+	_actuatorsSub(-1),
+	_armingSub(-1),
+	_parameterSub(-1),
+	_vehicleGlobalPosition({}),
+	_sensors({}),
+	_waypoint({}),
+	_actuators({}),
+	_arming({}),
+	_landTimer(0)
+{
+	_paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX");
+	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
+	_paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX");
+	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
+}
+
+void MulticopterLandDetector::initialize()
+{
+	// subscribe to position, attitude, arming and velocity changes
+	_vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined));
+	_waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
+	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
+
+	// download parameters
+	updateParameterCache(true);
+}
+
+void MulticopterLandDetector::updateSubscriptions()
+{
+	orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition);
+	orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors);
+	orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint);
+	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
+	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+}
+
+bool MulticopterLandDetector::update()
+{
+	// first poll for new data from our subscriptions
+	updateSubscriptions();
+
+	// only trigger flight conditions if we are armed
+	if (!_arming.armed) {
+		return true;
+	}
+
+	const uint64_t now = hrt_absolute_time();
+
+	// check if we are moving vertically
+	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
+
+	// check if we are moving horizontally
+	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+					+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity;
+
+	// next look if all rotation angles are not moving
+	bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] +
+			      _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] +
+			      _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation;
+
+	// check if thrust output is minimal (about half of default)
+	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
+
+	if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
+		// sensed movement, so reset the land detector
+		_landTimer = now;
+		return false;
+	}
+
+	return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME;
+}
+
+void MulticopterLandDetector::updateParameterCache(const bool force)
+{
+	bool updated;
+	parameter_update_s paramUpdate;
+
+	orb_check(_parameterSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+	}
+
+	if (updated || force) {
+		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+		param_get(_paramHandle.maxRotation, &_params.maxRotation);
+		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
+	}
+}
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
new file mode 100644
index 000000000..7bb7f1a90
--- /dev/null
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file MulticopterLandDetector.h
+ * Land detection algorithm for multicopters
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ */
+
+#ifndef __MULTICOPTER_LAND_DETECTOR_H__
+#define __MULTICOPTER_LAND_DETECTOR_H__
+
+#include "LandDetector.h"
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+
+class MulticopterLandDetector : public LandDetector
+{
+public:
+	MulticopterLandDetector();
+
+protected:
+	/**
+	* @brief  polls all subscriptions and pulls any data that has changed
+	**/
+	void updateSubscriptions();
+
+	/**
+	* @brief Runs one iteration of the land detection algorithm
+	**/
+	bool update() override;
+
+	/**
+	* @brief Initializes the land detection algorithm
+	**/
+	void initialize() override;
+
+
+private:
+	/**
+	* @brief download and update local parameter cache
+	**/
+	void updateParameterCache(const bool force);
+
+	/**
+	* @brief Handles for interesting parameters
+	**/
+	struct {
+		param_t maxClimbRate;
+		param_t maxVelocity;
+		param_t maxRotation;
+		param_t maxThrottle;
+	}		_paramHandle;
+
+	struct {
+		float maxClimbRate;
+		float maxVelocity;
+		float maxRotation;
+		float maxThrottle;
+	} _params;
+
+private:
+	int _vehicleGlobalPositionSub;                                      /**< notification of global position */
+	int _sensorsCombinedSub;
+	int _waypointSub;
+	int _actuatorsSub;
+	int _armingSub;
+	int _parameterSub;
+
+	struct vehicle_global_position_s          _vehicleGlobalPosition;   /**< the result from global position subscription */
+	struct sensor_combined_s                  _sensors;                 /**< subscribe to sensor readings */
+	struct position_setpoint_triplet_s        _waypoint;                /**< subscribe to autopilot navigation */
+	struct actuator_controls_s                _actuators;
+	struct actuator_armed_s                   _arming;
+
+	uint64_t _landTimer;                                                /**< timestamp in microseconds since a possible land was detected*/
+};
+
+#endif //__MULTICOPTER_LAND_DETECTOR_H__
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
new file mode 100644
index 000000000..1e43e7ad5
--- /dev/null
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file land_detector_main.cpp
+ * Land detection algorithm
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#include <unistd.h>					//usleep
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/systemlib.h>	//Scheduler
+#include <systemlib/err.h>			//print to console
+
+#include "FixedwingLandDetector.h"
+#include "MulticopterLandDetector.h"
+
+//Function prototypes
+static int land_detector_start(const char *mode);
+static void land_detector_stop();
+
+/**
+ * land detector app start / stop handling function
+ * This makes the land detector module accessible from the nuttx shell
+ * @ingroup apps
+ */
+extern "C" __EXPORT int land_detector_main(int argc, char *argv[]);
+
+//Private variables
+static LandDetector *land_detector_task = nullptr;
+static int _landDetectorTaskID = -1;
+static char _currentMode[12];
+
+/**
+* Deamon thread function
+**/
+static void land_detector_deamon_thread(int argc, char *argv[])
+{
+	land_detector_task->start();
+}
+
+/**
+* Stop the task, force killing it if it doesn't stop by itself
+**/
+static void land_detector_stop()
+{
+	if (land_detector_task == nullptr || _landDetectorTaskID == -1) {
+		errx(1, "not running");
+		return;
+	}
+
+	land_detector_task->shutdown();
+
+	//Wait for task to die
+	int i = 0;
+
+	do {
+		/* wait 20ms */
+		usleep(20000);
+
+		/* if we have given up, kill it */
+		if (++i > 50) {
+			task_delete(_landDetectorTaskID);
+			break;
+		}
+	} while (land_detector_task->isRunning());
+
+
+	delete land_detector_task;
+	land_detector_task = nullptr;
+	_landDetectorTaskID = -1;
+	errx(0, "land_detector has been stopped");
+}
+
+/**
+* Start new task, fails if it is already running. Returns OK if successful
+**/
+static int land_detector_start(const char *mode)
+{
+	if (land_detector_task != nullptr || _landDetectorTaskID != -1) {
+		errx(1, "already running");
+		return -1;
+	}
+
+	//Allocate memory
+	if (!strcmp(mode, "fixedwing")) {
+		land_detector_task = new FixedwingLandDetector();
+
+	} else if (!strcmp(mode, "multicopter")) {
+		land_detector_task = new MulticopterLandDetector();
+
+	} else {
+		errx(1, "[mode] must be either 'fixedwing' or 'multicopter'");
+		return -1;
+	}
+
+	//Check if alloc worked
+	if (land_detector_task == nullptr) {
+		errx(1, "alloc failed");
+		return -1;
+	}
+
+	//Start new thread task
+	_landDetectorTaskID = task_spawn_cmd("land_detector",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_DEFAULT,
+					     1200,
+					     (main_t)&land_detector_deamon_thread,
+					     nullptr);
+
+	if (_landDetectorTaskID < 0) {
+		errx(1, "task start failed: %d", -errno);
+		return -1;
+	}
+
+	/* avoid memory fragmentation by not exiting start handler until the task has fully started */
+	const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
+
+	while (!land_detector_task->isRunning()) {
+		usleep(50000);
+		printf(".");
+		fflush(stdout);
+
+		if (hrt_absolute_time() > timeout) {
+			err(1, "start failed - timeout");
+			land_detector_stop();
+			exit(1);
+		}
+	}
+
+	printf("\n");
+
+	//Remember current active mode
+	strncpy(_currentMode, mode, 12);
+
+	exit(0);
+	return 0;
+}
+
+/**
+* Main entry point for this module
+**/
+int land_detector_main(int argc, char *argv[])
+{
+
+	if (argc < 1) {
+		warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
+		exit(0);
+	}
+
+	if (argc >= 2 && !strcmp(argv[1], "start")) {
+		land_detector_start(argv[2]);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		land_detector_stop();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (land_detector_task) {
+
+			if (land_detector_task->isRunning()) {
+				warnx("running (%s)", _currentMode);
+
+			} else {
+				errx(1, "exists, but not running (%s)", _currentMode);
+			}
+
+			exit(0);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
+	return 1;
+}
diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c
new file mode 100644
index 000000000..0302bc7c1
--- /dev/null
+++ b/src/modules/land_detector/land_detector_params.c
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file land_detector.c
+ * Land detector algorithm parameters.
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Multicopter max climb rate
+ *
+ * Maximum vertical velocity allowed to trigger a land (m/s up and down)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f);
+
+/**
+ * Multicopter max horizontal velocity
+ *
+ * Maximum horizontal velocity allowed to trigger a land (m/s)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
+
+/**
+ * Multicopter max rotation
+ *
+ * Maximum allowed around each axis to trigger a land (radians per second)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f);
+
+/**
+ * Multicopter max throttle
+ *
+ * Maximum actuator output on throttle before triggering a land
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
+
+/**
+ * Fixedwing max horizontal velocity
+ *
+ * Maximum horizontal velocity allowed to trigger a land (m/s)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f);
+
+/**
+ * Fixedwing max climb rate
+ *
+ * Maximum vertical velocity allowed to trigger a land (m/s up and down)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f);
+
+/**
+ * Airspeed max
+ *
+ * Maximum airspeed allowed to trigger a land (m/s)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f);
diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk
new file mode 100644
index 000000000..e08a4b7a8
--- /dev/null
+++ b/src/modules/land_detector/module.mk
@@ -0,0 +1,13 @@
+#
+# Land detector
+#
+
+MODULE_COMMAND	= land_detector
+
+SRCS		= land_detector_main.cpp \
+              land_detector_params.c \
+              LandDetector.cpp \
+			  MulticopterLandDetector.cpp \
+              FixedwingLandDetector.cpp
+
+EXTRACXXFLAGS   = -Weffc++ -Os
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 97108270c..dfbf00b66 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),
 	status{},
 	hil_local_pos{},
+	hil_land_detector{},
 	_control_mode{},
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
@@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
+	_land_detector_pub(-1),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -1353,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.xy_global = true;
 		hil_local_pos.z_global = true;
 
-		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
-		hil_local_pos.landed = landed;
-
 		if (_local_pos_pub < 0) {
 			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
@@ -1364,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		}
 	}
 
+	/* land detector */
+	{
+		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
+		if(hil_land_detector.landed != landed) {
+			hil_land_detector.landed = landed;
+			hil_land_detector.timestamp = hrt_absolute_time();
+
+			if (_land_detector_pub < 0) {
+				_land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector);
+
+			} else {
+				orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector);
+			}
+		}
+	}
+
 	/* accelerometer */
 	{
 		struct accel_report accel;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 4afc9b372..699996860 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -50,6 +50,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
@@ -145,6 +146,7 @@ private:
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
+	struct vehicle_land_detected_s hil_land_detector;
 	struct vehicle_control_mode_s _control_mode;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
@@ -171,6 +173,7 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	orb_advert_t _land_detector_pub;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 2f972fc9f..39294e3c0 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -246,9 +246,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
 	float surface_offset = 0.0f;	// ground level offset from reference altitude
 	float surface_offset_rate = 0.0f;	// surface offset change rate
-	float alt_avg = 0.0f;
-	bool landed = true;
-	hrt_abstime landed_time = 0;
 
 	hrt_abstime accel_timestamp = 0;
 	hrt_abstime baro_timestamp = 0;
@@ -1068,36 +1065,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
 		}
 
-		/* detect land */
-		alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
-		float alt_disp2 = - z_est[0] - alt_avg;
-		alt_disp2 = alt_disp2 * alt_disp2;
-		float land_disp2 = params.land_disp * params.land_disp;
-		/* get actual thrust output */
-		float thrust = armed.armed ? actuator.control[3] : 0.0f;
-
-		if (landed) {
-			if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
-				landed = false;
-				landed_time = 0;
-			}
-		} else {
-			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
-				if (landed_time == 0) {
-					landed_time = t;    // land detected first time
-
-				} else {
-					if (t > landed_time + params.land_t * 1000000.0f) {
-						landed = true;
-						landed_time = 0;
-					}
-				}
-
-			} else {
-				landed_time = 0;
-			}
-		}
-
 		if (verbose_mode) {
 			/* print updates rate */
 			if (t > updates_counter_start + updates_counter_len) {
@@ -1148,7 +1115,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.vy = y_est[1];
 			local_pos.z = z_est[0];
 			local_pos.vz = z_est[1];
-			local_pos.landed = landed;
 			local_pos.yaw = att.yaw;
 			local_pos.dist_bottom_valid = dist_bottom_valid;
 			local_pos.eph = eph;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index d35b70239..7b7949239 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1112,6 +1112,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
 	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
 	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
+	
 	/* we need to rate-limit wind, as we do not need the full update rate */
 	orb_set_interval(subs.wind_sub, 90);
 	subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
@@ -1514,7 +1515,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 											  (buf.local_pos.v_z_valid ? 8 : 0) |
 											  (buf.local_pos.xy_global ? 16 : 0) |
 											  (buf.local_pos.z_global ? 32 : 0);
-			log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 			log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
 			log_msg.body.log_LPOS.eph = buf.local_pos.eph;
 			log_msg.body.log_LPOS.epv = buf.local_pos.epv;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 5941bfac0..99f70a948 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -113,7 +113,6 @@ struct log_LPOS_s {
 	int32_t ref_lon;
 	float ref_alt;
 	uint8_t pos_flags;
-	uint8_t landed;
 	uint8_t ground_dist_flags;
 	float eph;
 	float epv;
@@ -427,6 +426,7 @@ struct log_ENCD_s {
 /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
 #define LOG_AIR1_MSG 40
 
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -462,7 +462,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(IMU2, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT_S(SENS, SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
 	LOG_FORMAT_S(AIR1, SENS, "fffff",	"BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"),
-	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
+	LOG_FORMAT(LPOS, "ffffffffLLfBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 293412455..78fdf4de7 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -83,6 +83,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
 #include "topics/vehicle_gps_position.h"
 ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
 
+#include "topics/vehicle_land_detected.h"
+ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s);
+
 #include "topics/satellite_info.h"
 ORB_DEFINE(satellite_info, struct satellite_info_s);
 
diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h
new file mode 100644
index 000000000..51b3568e8
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_land_detected.h
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_land_detected.h
+ * Land detected uORB topic
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#ifndef __TOPIC_VEHICLE_LANDED_H__
+#define __TOPIC_VEHICLE_LANDED_H__
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_land_detected_s {
+	uint64_t timestamp;	   /**< timestamp of the setpoint */
+	bool landed;           /**< true if vehicle is currently landed on the ground*/
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_land_detected);
+
+#endif //__TOPIC_VEHICLE_LANDED_H__
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 5d39c897d..8b46c5a3f 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -77,7 +77,6 @@ struct vehicle_local_position_s {
 	double ref_lat;		/**< Reference point latitude in degrees */
 	double ref_lon;		/**< Reference point longitude in degrees */
 	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
-	bool landed;			/**< true if vehicle is landed */
 	/* Distance to surface */
 	float dist_bottom;		/**< Distance to bottom surface (ground) */
 	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */