land_detector: big refactor, share update function

This is a big refactor and general cleanup of the land detector. The
main functional change is to share the hysteresis lib across all land
detectors.

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -31,22 +31,23 @@
  *
  ****************************************************************************/
 
-/**
+/*
  * @file FixedwingLandDetector.cpp
- * Land detection algorithm for fixedwings
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "FixedwingLandDetector.h"
-
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <cmath>
 #include <drivers/drv_hrt.h>
 
-namespace landdetection
+#include "FixedwingLandDetector.h"
+
+
+namespace land_detector
 {
 
 FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
@@ -58,12 +59,10 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_controlState{},
 	_arming{},
 	_airspeed{},
-	_parameterSub(-1),
 	_velocity_xy_filtered(0.0f),
 	_velocity_z_filtered(0.0f),
 	_airspeed_filtered(0.0f),
-	_accel_horz_lp(0.0f),
-	_landDetectTrigger(0)
+	_accel_horz_lp(0.0f)
 {
 	_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
@@ -71,54 +70,41 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
 }
 
-void FixedwingLandDetector::initialize()
+void FixedwingLandDetector::_initialize_topics()
 {
 	_controlStateSub = orb_subscribe(ORB_ID(control_state));
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
-
-	updateParameterCache(true);
 }
 
-void FixedwingLandDetector::updateSubscriptions()
+void FixedwingLandDetector::_update_topics()
 {
-	orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
-	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
-	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	_orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
+	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
-LandDetectionResult FixedwingLandDetector::update()
+void FixedwingLandDetector::_update_params()
 {
-	// First poll for new data from our subscriptions
-	updateSubscriptions();
-
-	if (get_freefall_state()) {
-		_state = LANDDETECTION_RES_FREEFALL;
-
-	} else if (get_landed_state()) {
-		_state = LANDDETECTION_RES_LANDED;
-
-	} else {
-		_state = LANDDETECTION_RES_FLYING;
-	}
-
-	return _state;
+	param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+	param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+	param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
+	param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
 }
 
-bool FixedwingLandDetector::get_freefall_state()
+bool FixedwingLandDetector::_get_freefall_state()
 {
-	//TODO
+	// TODO
 	return false;
 }
 
-bool FixedwingLandDetector::get_landed_state()
+bool FixedwingLandDetector::_get_landed_state()
 {
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
 		return true;
 	}
 
-	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;
 
 	if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
@@ -147,14 +133,10 @@ bool FixedwingLandDetector::get_landed_state()
 		    && _airspeed_filtered < _params.maxAirSpeed
 		    && _accel_horz_lp < _params.maxIntVelocity) {
 
-			// these conditions need to be stable for a period of time before we trust them
-			if (now > _landDetectTrigger) {
-				landDetected = true;
-			}
+			landDetected = true;
 
 		} else {
-			// reset land detect trigger
-			_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
+			landDetected = false;
 		}
 
 	} else {
@@ -165,23 +147,4 @@ bool FixedwingLandDetector::get_landed_state()
 	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);
-		param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
-	}
-}
-
-}
+} // namespace land_detector

src/modules/land_detector/FixedwingLandDetector.h

@@ -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
@@ -33,23 +33,22 @@
 
 /**
  * @file FixedwingLandDetector.h
- * Land detection algorithm for fixedwing
+ * Land detector implementation for fixedwing.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __FIXED_WING_LAND_DETECTOR_H__
-#define __FIXED_WING_LAND_DETECTOR_H__
+#pragma once
 
-#include "LandDetector.h"
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/airspeed.h>
-#include <systemlib/param/param.h>
 
-namespace landdetection
+#include "LandDetector.h"
+
+namespace land_detector
 {
 
 class FixedwingLandDetector : public LandDetector
@@ -58,46 +57,22 @@ public:
 	FixedwingLandDetector();
 
 protected:
-	/**
-	* @brief  blocking loop, should be run in a separate thread or task. Runs at 50Hz
-	**/
-	LandDetectionResult update() override;
+	virtual void _initialize_topics() override;
 
-	/**
-	* @brief Initializes the land detection algorithm
-	**/
-	void initialize() override;
+	virtual void _update_params() override;
 
-	/**
-	* @brief  polls all subscriptions and pulls any data that has changed
-	**/
-	void updateSubscriptions();
+	virtual void _update_topics() override;
 
-	/**
-	* @brief get UAV landed state
-	**/
-	bool get_landed_state();
-
-	/**
-	* @brief returns true if UAV is in free-fall state
-	**/
-	bool get_freefall_state();
+	virtual bool _get_landed_state() override;
 
+	virtual bool _get_freefall_state() override;
 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;
 		param_t maxIntVelocity;
-	}		_paramHandle;
+	} _paramHandle;
 
 	struct {
 		float maxVelocity;
@@ -106,22 +81,18 @@ private:
 		float maxIntVelocity;
 	} _params;
 
-private:
-	int					_controlStateSub;	/**< notification of local position */
-	int					_armingSub;
-	int					_airspeedSub;
-	struct control_state_s			_controlState;		/**< the result from local position subscription */
-	struct actuator_armed_s			_arming;
-	struct airspeed_s			_airspeed;
-	int 					_parameterSub;
+	int _controlStateSub;
+	int _armingSub;
+	int _airspeedSub;
+
+	struct control_state_s _controlState;
+	struct actuator_armed_s _arming;
+	struct airspeed_s _airspeed;
 
 	float _velocity_xy_filtered;
 	float _velocity_z_filtered;
 	float _airspeed_filtered;
 	float _accel_horz_lp;
-	uint64_t _landDetectTrigger;
 };
 
-}
-
-#endif //__FIXED_WING_LAND_DETECTOR_H__
+} // namespace land_detector

src/modules/land_detector/LandDetector.cpp

@@ -31,32 +31,35 @@
  *
  ****************************************************************************/
 
-/**
+/*
  * @file LandDetector.cpp
- * Land detection algorithm
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
- * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "LandDetector.h"
-#include <unistd.h>                 //usleep
-#include <drivers/drv_hrt.h>
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <drivers/drv_hrt.h>
 
-namespace landdetection
+#include "LandDetector.h"
+
+
+namespace land_detector
 {
 
+
 LandDetector::LandDetector() :
 	_landDetectedPub(nullptr),
 	_landDetected{0, false, false},
-	_arming_time(0),
+	_freefall_hysteresis(false),
+	_landed_hysteresis(true),
 	_taskShouldExit(false),
 	_taskIsRunning(false),
 	_work{}
 {
-	// ctor
+	// Use Trigger time when transitioning from in-air (false) to landed (true).
+	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
 }
 
 LandDetector::~LandDetector()
@@ -67,60 +70,115 @@ LandDetector::~LandDetector()
 
 int LandDetector::start()
 {
+	_taskShouldExit = false;
+
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
+	work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0);
 
 	return 0;
 }
 
-void LandDetector::shutdown()
+void LandDetector::stop()
 {
 	_taskShouldExit = true;
 }
 
 void
-LandDetector::cycle_trampoline(void *arg)
+LandDetector::_cycle_trampoline(void *arg)
 {
 	LandDetector *dev = reinterpret_cast<LandDetector *>(arg);
 
-	dev->cycle();
+	dev->_cycle();
 }
 
-void LandDetector::cycle()
+void LandDetector::_cycle()
 {
 	if (!_taskIsRunning) {
-		// advertise the first land detected uORB
+		PX4_INFO("we are doing the init");
+		// Advertise the first land detected uORB.
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = false;
 		_landDetected.freefall = false;
 
-		// initialize land detection algorithm
-		initialize();
+		// Initialize uORB topics.
+		_initialize_topics();
 
-		// task is now running, keep doing so until shutdown() has been called
+		_check_params(true);
+
+		// Task is now running, keep doing so until we need to stop.
 		_taskIsRunning = true;
-		_taskShouldExit = false;
 	}
 
-	LandDetectionResult current_state = update();
-	bool landDetected = (current_state == LANDDETECTION_RES_LANDED);
-	bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL);
+	_check_params(false);
+
+	_update_topics();
+
+	_update_state();
+
+	bool landDetected = (_state == LandDetectionState::LANDED);
+	bool freefallDetected = (_state == LandDetectionState::FREEFALL);
+
+	// Only publish very first time or when the result has changed.
+	if ((_landDetectedPub == nullptr) ||
+	    (_landDetected.landed != landDetected) ||
+	    (_landDetected.freefall != freefallDetected)) {
 
-	if (_landDetectedPub == nullptr || _landDetected.landed != landDetected || _landDetected.freefall != freefallDetected) {
 		_landDetected.timestamp = hrt_absolute_time();
-		_landDetected.landed = (current_state == LANDDETECTION_RES_LANDED);
-		_landDetected.freefall = (current_state == LANDDETECTION_RES_FREEFALL);
+		_landDetected.landed = (_state == LandDetectionState::LANDED);
+		_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
+
 		int instance;
-		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, &instance, ORB_PRIO_DEFAULT);
+		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
+				 &instance, ORB_PRIO_DEFAULT);
 	}
 
 	if (!_taskShouldExit) {
-		work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
-			   USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
+
+		// Schedule next cycle.
+		work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this,
+			   USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ));
+
+	} else {
+		_taskIsRunning = false;
 	}
 }
 
-bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
+void LandDetector::_check_params(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) {
+		_update_params();
+	}
+}
+
+void LandDetector::_update_state()
+{
+	bool landed = _get_landed_state();
+	_landed_hysteresis.set_state_and_update(landed);
+	_freefall_hysteresis.set_state_and_update(_get_freefall_state());
+
+	if (_freefall_hysteresis.get_state()) {
+		_state = LandDetectionState::FREEFALL;
+
+	} else if (_landed_hysteresis.get_state()) {
+		_state = LandDetectionState::LANDED;
+
+	} else {
+		_state = LandDetectionState::FLYING;
+	}
+
+	return;
+}
+
+bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer)
 {
 	bool newData = false;
 
@@ -140,4 +198,5 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void
 	return true;
 }
 
-}
+
+} // namespace land_detector

src/modules/land_detector/LandDetector.h

@@ -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
@@ -31,101 +31,131 @@
  *
  ****************************************************************************/
 
-/**
+/*
  * @file LandDetector.h
- * Abstract interface for land detector algorithms
+Land detector interface for multicopter, fixedwing and VTOL implementations.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __LAND_DETECTOR_H__
-#define __LAND_DETECTOR_H__
+#pragma once
 
 #include <px4_workqueue.h>
+#include <systemlib/hysteresis/hysteresis.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_land_detected.h>
 
-namespace landdetection
+namespace land_detector
 {
 
-enum LandDetectionResult {
-	LANDDETECTION_RES_FLYING = 0,	/**< UAV is flying */
-	LANDDETECTION_RES_LANDED = 1,	/**< Land has been detected */
-	LANDDETECTION_RES_FREEFALL = 2	/**< Free-fall has been detected */
-};
 
 class LandDetector
 {
 public:
+	enum class LandDetectionState {
+		FLYING = 0,
+		LANDED = 1,
+		FREEFALL = 2
+	};
 
 	LandDetector();
 	virtual ~LandDetector();
 
-	/**
-	 * @return true if this task is currently running
-	 **/
-	inline bool isRunning() const {return _taskIsRunning;}
-
-	/**
-	 * @return the current landed state
+	/*
+	 * @return true if this task is currently running.
 	 */
-	bool isLanded() { return _landDetected.landed; }
+	inline bool is_running() 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.
-	 **/
+	/*
+	 * @return current state.
+	 */
+	LandDetectionState get_state() const
+	{
+		return _state;
+	}
+
+	/*
+	 * Tells the task that it should exit.
+	 */
+	void stop();
+
+	/*
+	 * Get the work queue going.
+	 */
 	int start();
 
-	static void	cycle_trampoline(void *arg);
-
 protected:
+	/*
+	 * Called once to initialize uORB topics.
+	 */
+	virtual void _initialize_topics() = 0;
 
-	/**
-	* @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 LandDetectionResult update() = 0;
+	/*
+	 * Update uORB topics.
+	 */
+	virtual void _update_topics() = 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 Convenience 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);
+	/*
+	 * Update parameters.
+	 */
+	virtual void _update_params() = 0;
 
-	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50;        /**< Run algorithm at 50Hz */
+	/*
+	 * @return true if UAV is in a landed state.
+	 */
+	virtual bool _get_landed_state() = 0;
 
-	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
-                                                                          before triggering a land */
-	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME =
-		2000000;	/**< time interval in which wider acceptance thresholds are used after arming */
+	/*
+	 * @return true if UAV is in free-fall state.
+	 */
+	virtual bool _get_freefall_state() = 0;
 
-protected:
-	orb_advert_t				_landDetectedPub;		/**< publisher for position in local frame */
-	struct vehicle_land_detected_s		_landDetected;			/**< local vehicle position */
-	uint64_t				_arming_time;			/**< timestamp of arming time */
-	LandDetectionResult
-	_state;		/**< Result of land detection. Can be LANDDETECTION_RES_FLYING, LANDDETECTION_RES_LANDED or LANDDETECTION_RES_FREEFALL */
+	/*
+	 * Convenience function for polling uORB subscriptions.
+	 *
+	 * @return true if there was new data and it was successfully copied
+	 */
+	static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer);
+
+	/* Run main land detector loop at this rate in Hz. */
+	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
+
+	/* Time in us that landing conditions have to hold before triggering a land. */
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000;
+
+	/* Time interval in us in which wider acceptance thresholds are used after arming. */
+	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000;
+
+	orb_advert_t _landDetectedPub;
+	struct vehicle_land_detected_s _landDetected;
+
+	int _parameterSub;
+
+	LandDetectionState _state;
+
+	systemlib::Hysteresis _freefall_hysteresis;
+	systemlib::Hysteresis _landed_hysteresis;
 
 private:
-	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */
-	bool _taskIsRunning;                                                /**< task has reached main loop and is currently running */
-	struct work_s	_work;
+	static void _cycle_trampoline(void *arg);
 
-	void		cycle();
+	void _cycle();
+
+	void _check_params(const bool force);
+
+	void _update_state();
+
+	bool _taskShouldExit;
+	bool _taskIsRunning;
+
+	struct work_s	_work;
 };
 
-}
 
-#endif //__LAND_DETECTOR_H__
+} // namespace land_detector

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -33,19 +33,20 @@
 
 /**
  * @file MulticopterLandDetector.cpp
- * Land detection algorithm for multicopters
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "MulticopterLandDetector.h"
-
 #include <cmath>
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 
-namespace landdetection
+#include "MulticopterLandDetector.h"
+
+
+namespace land_detector
 {
 
 MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
@@ -54,7 +55,6 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_vehicleLocalPositionSub(-1),
 	_actuatorsSub(-1),
 	_armingSub(-1),
-	_parameterSub(-1),
 	_attitudeSub(-1),
 	_manualSub(-1),
 	_ctrl_state_sub(-1),
@@ -67,12 +67,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_ctrl_state{},
 	_ctrl_mode{},
 	_min_trust_start(0),
-	_freefall_hysteresis(false),
-	_landed_hysteresis(true)
+	_arming_time(0)
 {
-	// Use Trigger time when transitioning from in-air (false) to landed (true).
-	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME);
-
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
 	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
@@ -82,7 +78,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
 }
 
-void MulticopterLandDetector::initialize()
+void MulticopterLandDetector::_initialize_topics()
 {
 	// subscribe to position, attitude, arming and velocity changes
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -93,46 +89,34 @@ void MulticopterLandDetector::initialize()
 	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-
-	// download parameters
-	updateParameterCache(true);
 }
 
-void MulticopterLandDetector::updateSubscriptions()
+void MulticopterLandDetector::_update_topics()
 {
-	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
-	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
-	orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
-	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
-	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
-	orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
-	orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode);
+	_orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
+	_orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
+	_orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
+	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	_orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
+	_orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
+	_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode);
 }
 
-LandDetectionResult MulticopterLandDetector::update()
+void MulticopterLandDetector::_update_params()
 {
-	// first poll for new data from our subscriptions
-	updateSubscriptions();
-
-	updateParameterCache(false);
-
-	_landed_hysteresis.set_state_and_update(get_landed_state());
-	_freefall_hysteresis.set_state_and_update(get_freefall_state());
-
-	if (_freefall_hysteresis.get_state()) {
-		_state = LANDDETECTION_RES_FREEFALL;
-
-	} else if (_landed_hysteresis.get_state()) {
-		_state = LANDDETECTION_RES_LANDED;
-
-	} else {
-		_state = LANDDETECTION_RES_FLYING;
-	}
-
-	return _state;
+	param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+	param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+	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);
+	_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time);
 }
 
-bool MulticopterLandDetector::get_freefall_state()
+
+bool MulticopterLandDetector::_get_freefall_state()
 {
 	if (_params.acc_threshold_m_s2 < 0.1f
 	    || _params.acc_threshold_m_s2 > 10.0f) {	//if parameter is set to zero or invalid, disable free-fall detection.
@@ -152,7 +136,7 @@ bool MulticopterLandDetector::get_freefall_state()
 	return (acc_norm < _params.acc_threshold_m_s2);	//true if we are currently falling
 }
 
-bool MulticopterLandDetector::get_landed_state()
+bool MulticopterLandDetector::_get_landed_state()
 {
 	// Time base for this function
 	const uint64_t now = hrt_absolute_time();
@@ -177,6 +161,7 @@ bool MulticopterLandDetector::get_landed_state()
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
 		_arming_time = 0;
+
 		return true;
 
 	} else if (_arming_time == 0) {
@@ -203,6 +188,7 @@ bool MulticopterLandDetector::get_landed_state()
 		// quite acrobatic flight.
 		if ((_min_trust_start > 0) &&
 		    (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) {
+
 			return true;
 
 		} else {
@@ -214,7 +200,7 @@ bool MulticopterLandDetector::get_landed_state()
 
 	// 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) {
+	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) {
 		armThresholdFactor = 2.5f;
 	}
 
@@ -243,28 +229,5 @@ bool MulticopterLandDetector::get_landed_state()
 	return true;
 }
 
-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_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);
-		_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time);
-	}
-}
 
 }

src/modules/land_detector/MulticopterLandDetector.h

@@ -33,29 +33,28 @@
 
 /**
  * @file MulticopterLandDetector.h
- * Land detection algorithm for multicopters
+ * Land detection implementation for multicopters.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __MULTICOPTER_LAND_DETECTOR_H__
-#define __MULTICOPTER_LAND_DETECTOR_H__
+#pragma once
 
-#include "LandDetector.h"
+#include <systemlib/param/param.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
 #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 <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <systemlib/param/param.h>
-#include <systemlib/hysteresis/hysteresis.h>
 
-namespace landdetection
+#include "LandDetector.h"
+
+namespace land_detector
 {
 
 class MulticopterLandDetector : public LandDetector
@@ -64,36 +63,15 @@ public:
 	MulticopterLandDetector();
 
 protected:
-	/**
-	* @brief  polls all subscriptions and pulls any data that has changed
-	**/
-	virtual void updateSubscriptions();
+	virtual void _initialize_topics() override;
 
-	/**
-	* @brief Runs one iteration of the land detection algorithm
-	**/
-	virtual LandDetectionResult update() override;
+	virtual void _update_params() override;
 
-	/**
-	* @brief Initializes the land detection algorithm
-	**/
-	virtual void initialize() override;
+	virtual void _update_topics() override;
 
-	/**
-	* @brief download and update local parameter cache
-	**/
-	virtual void updateParameterCache(const bool force);
-
-	/**
-	* @brief get multicopter landed state
-	**/
-	bool get_landed_state();
-
-	/**
-	* @brief returns true if multicopter is in free-fall state
-	**/
-	bool get_freefall_state();
+	virtual bool _get_landed_state() override;
 
+	virtual bool _get_freefall_state() override;
 private:
 
 	/**
@@ -107,7 +85,7 @@ private:
 		param_t minManThrottle;
 		param_t acc_threshold_m_s2;
 		param_t ff_trigger_time;
-	}		_paramHandle;
+	} _paramHandle;
 
 	struct {
 		float maxClimbRate;
@@ -119,11 +97,9 @@ private:
 		float ff_trigger_time;
 	} _params;
 
-private:
 	int _vehicleLocalPositionSub;
 	int _actuatorsSub;
 	int _armingSub;
-	int _parameterSub;
 	int _attitudeSub;
 	int _manualSub;
 	int _ctrl_state_sub;
@@ -138,11 +114,8 @@ private:
 	struct vehicle_control_mode_s		_ctrl_mode;
 
 	uint64_t _min_trust_start;		///< timestamp when minimum trust was applied first
-
-	systemlib::Hysteresis _freefall_hysteresis;
-	systemlib::Hysteresis _landed_hysteresis;
+	uint64_t _arming_time;
 };
 
-}
 
-#endif //__MULTICOPTER_LAND_DETECTOR_H__
+} // namespace land_detector

src/modules/land_detector/VtolLandDetector.cpp

@@ -36,12 +36,14 @@
  * Land detection algorithm for VTOL
  *
  * @author Roman Bapst <bapstroma@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "VtolLandDetector.h"
 #include <drivers/drv_hrt.h>
 
-namespace landdetection
+#include "VtolLandDetector.h"
+
+namespace land_detector
 {
 
 VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
@@ -56,29 +58,24 @@ VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
 	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
 }
 
-void VtolLandDetector::initialize()
+void VtolLandDetector::_initialize_topics()
 {
-	MulticopterLandDetector::initialize();
+	MulticopterLandDetector::_initialize_topics();
+
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
-
-	// download parameters
-	updateParameterCache(true);
 }
 
-void VtolLandDetector::updateSubscriptions()
+void VtolLandDetector::_update_topics()
 {
-	MulticopterLandDetector::updateSubscriptions();
+	MulticopterLandDetector::_update_topics();
 
-	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
-LandDetectionResult VtolLandDetector::update()
+bool VtolLandDetector::_get_landed_state()
 {
-	updateSubscriptions();
-	updateParameterCache(false);
-
 	// this is returned from the mutlicopter land detector
-	bool landed = get_landed_state();
+	bool landed = MulticopterLandDetector::_get_landed_state();
 
 	// for vtol we additionally consider airspeed
 	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
@@ -97,27 +94,19 @@ LandDetectionResult VtolLandDetector::update()
 
 	_was_in_air = !landed;
 
-	_state = (landed) ? LANDDETECTION_RES_LANDED : LANDDETECTION_RES_FLYING;
-
-	return _state;
+	return landed;
 }
 
-void VtolLandDetector::updateParameterCache(const bool force)
+bool VtolLandDetector::_get_freefall_state()
 {
-	MulticopterLandDetector::updateParameterCache(force);
+	return MulticopterLandDetector::_get_freefall_state();
+}
 
-	bool updated;
-	parameter_update_s paramUpdate;
+void VtolLandDetector::_update_params()
+{
+	MulticopterLandDetector::_update_params();
 
-	orb_check(_parameterSub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
-	}
-
-	if (updated || force) {
-		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
-	}
+	param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
 }
 
 }

src/modules/land_detector/VtolLandDetector.h

@@ -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
@@ -33,18 +33,19 @@
 
 /**
  * @file VtolLandDetector.h
- * Land detection algorithm for vtol
+ * Land detection implementation for VTOL also called hybrids.
  *
  * @author Roman Bapst <bapstr@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __VTOL_LAND_DETECTOR_H__
-#define __VTOL_LAND_DETECTOR_H__
+#pragma once
 
-#include "MulticopterLandDetector.h"
 #include <uORB/topics/airspeed.h>
 
-namespace landdetection
+#include "MulticopterLandDetector.h"
+
+namespace land_detector
 {
 
 class VtolLandDetector : public MulticopterLandDetector
@@ -52,34 +53,21 @@ class VtolLandDetector : public MulticopterLandDetector
 public:
 	VtolLandDetector();
 
+protected:
+	virtual void _initialize_topics() override;
+
+	virtual void _update_params() override;
+
+	virtual void _update_topics() override;
+
+	virtual bool _get_landed_state() override;
+
+	virtual bool _get_freefall_state() override;
 
 private:
-	/**
-	* @brief  polls all subscriptions and pulls any data that has changed
-	**/
-	void updateSubscriptions() override;
-
-	/**
-	* @brief Runs one iteration of the land detection algorithm
-	**/
-	LandDetectionResult update() override;
-
-	/**
-	* @brief Initializes the land detection algorithm
-	**/
-	void initialize() override;
-
-	/**
-	* @brief download and update local parameter cache
-	**/
-	void updateParameterCache(const bool force) override;
-
-	/**
-	* @brief Handles for interesting parameters
-	**/
 	struct {
 		param_t maxAirSpeed;
-	}		_paramHandle;
+	} _paramHandle;
 
 	struct {
 		float maxAirSpeed;
@@ -94,6 +82,5 @@ private:
 	float _airspeed_filtered; /**< low pass filtered airspeed */
 };
 
-#endif
 
-}
+} // namespace land_detector

src/modules/land_detector/land_detector_main.cpp

@@ -55,7 +55,8 @@
 #include "MulticopterLandDetector.h"
 #include "VtolLandDetector.h"
 
-namespace landdetection
+
+namespace land_detector
 {
 
 //Function prototypes
@@ -83,7 +84,7 @@ static void land_detector_stop()
 		return;
 	}
 
-	land_detector_task->shutdown();
+	land_detector_task->stop();
 
 	// Wait for task to die
 	int i = 0;
@@ -92,7 +93,7 @@ static void land_detector_stop()
 		/* wait 20ms */
 		usleep(20000);
 
-	} while (land_detector_task->isRunning() && ++i < 50);
+	} while (land_detector_task->is_running() && ++i < 50);
 
 
 	delete land_detector_task;
@@ -146,8 +147,8 @@ static int land_detector_start(const char *mode)
 	usleep(10000);
 
 	/* check if the waiting involving dots and a newline are still needed */
-	if (!land_detector_task->isRunning()) {
-		while (!land_detector_task->isRunning()) {
+	if (!land_detector_task->is_running()) {
+		while (!land_detector_task->is_running()) {
 			usleep(50000);
 
 			if (hrt_absolute_time() > timeout) {
@@ -191,8 +192,27 @@ int land_detector_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (land_detector_task) {
 
-			if (land_detector_task->isRunning()) {
-				PX4_WARN("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR");
+			if (land_detector_task->is_running()) {
+				PX4_INFO("running (%s): %s", _currentMode);
+				LandDetector::LandDetectionState state = land_detector_task->get_state();
+
+				switch (state) {
+				case LandDetector::LandDetectionState::FLYING:
+					PX4_INFO("State: Flying");
+					break;
+
+				case LandDetector::LandDetectionState::LANDED:
+					PX4_INFO("State: Landed");
+					break;
+
+				case LandDetector::LandDetectionState::FREEFALL:
+					PX4_INFO("State: Freefall");
+					break;
+
+				default:
+					PX4_ERR("State: unknown");
+					break;
+				}
 
 			} else {
 				PX4_WARN("exists, but not running (%s)", _currentMode);