Added room in land_detector code for free-fall detection

msg/vehicle_land_detected.msg

@@ -1,2 +1,3 @@
 uint64 timestamp	# timestamp of the setpoint
 bool landed		# true if vehicle is currently landed on the ground
+bool freefall	# true if vehicle is currently in free-fall

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -46,6 +46,9 @@
 #include <cmath>
 #include <drivers/drv_hrt.h>
 
+namespace landdetection
+{
+
 FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_paramHandle(),
 	_params(),
@@ -84,11 +87,30 @@ void FixedwingLandDetector::updateSubscriptions()
 	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
-bool FixedwingLandDetector::update()
+LandDetectionResult FixedwingLandDetector::update()
 {
 	// 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;
+}
+
+bool FixedwingLandDetector::get_freefall_state()
+{
+	//TODO
+	return false;
+}
+
+bool FixedwingLandDetector::get_landed_state()
+{
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
 		return true;
@@ -159,3 +181,5 @@ void FixedwingLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
 	}
 }
+
+}

src/modules/land_detector/FixedwingLandDetector.h

@@ -49,6 +49,9 @@
 #include <uORB/topics/airspeed.h>
 #include <systemlib/param/param.h>
 
+namespace landdetection
+{
+
 class FixedwingLandDetector : public LandDetector
 {
 public:
@@ -58,7 +61,7 @@ protected:
 	/**
 	* @brief  blocking loop, should be run in a separate thread or task. Runs at 50Hz
 	**/
-	bool update() override;
+	LandDetectionResult update() override;
 
 	/**
 	* @brief Initializes the land detection algorithm
@@ -70,6 +73,16 @@ protected:
 	**/
 	void updateSubscriptions();
 
+	/**
+	* @brief get UAV landed state
+	**/
+	bool get_landed_state();
+
+	/**
+	* @brief returns true if UAV is in free-fall state
+	**/
+	bool get_freefall_state();
+
 private:
 	/**
 	* @brief download and update local parameter cache
@@ -109,4 +122,6 @@ private:
 	uint64_t _landDetectTrigger;
 };
 
+}
+
 #endif //__FIXED_WING_LAND_DETECTOR_H__

src/modules/land_detector/LandDetector.cpp

@@ -45,6 +45,9 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 
+namespace landdetection
+{
+
 LandDetector::LandDetector() :
 	_landDetectedPub(0),
 	_landDetected( {0, false}),
@@ -88,6 +91,7 @@ void LandDetector::cycle()
 		// advertise the first land detected uORB
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = false;
+		_landDetected.freefall = false;
 		_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
 
 		// initialize land detection algorithm
@@ -98,12 +102,15 @@ void LandDetector::cycle()
 		_taskShouldExit = false;
 	}
 
-	bool landDetected = update();
+	LandDetectionResult current_state = update();
+	bool landDetected = (current_state==LANDDETECTION_RES_LANDED);
+	bool freefallDetected = (current_state==LANDDETECTION_RES_FREEFALL);
 
 	// publish if land detection state has changed
-	if (_landDetected.landed != landDetected) {
+	if ((_landDetected.landed != landDetected) || (_landDetected.freefall != freefallDetected)) {
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = landDetected;
+		_landDetected.freefall = freefallDetected;
 
 		// publish the land detected broadcast
 		orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
@@ -135,3 +142,5 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void
 
 	return true;
 }
+
+}

src/modules/land_detector/LandDetector.h

@@ -45,6 +45,15 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_land_detected.h>
 
+namespace landdetection
+{
+
+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:
@@ -81,7 +90,7 @@ 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;
+	virtual LandDetectionResult update() = 0;
 
 	/**
 	* @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation,
@@ -106,6 +115,7 @@ 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 */
 
 private:
 	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */
@@ -115,4 +125,6 @@ private:
 	void		cycle();
 };
 
+}
+
 #endif //__LAND_DETECTOR_H__

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -45,6 +45,9 @@
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 
+namespace landdetection
+{
+
 MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle(),
 	_params(),
@@ -89,14 +92,28 @@ void MulticopterLandDetector::updateSubscriptions()
 	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
 }
 
-bool MulticopterLandDetector::update()
+LandDetectionResult MulticopterLandDetector::update()
 {
 	// first poll for new data from our subscriptions
 	updateSubscriptions();
 
 	updateParameterCache(false);
 
-	return get_landed_state();
+	if (get_freefall_state()) {
+		_state = LANDDETECTION_RES_FREEFALL;
+	}else if(get_landed_state()){
+		_state = LANDDETECTION_RES_LANDED;
+	}else{
+		_state = LANDDETECTION_RES_FLYING;
+	}
+
+	return _state;
+}
+
+bool MulticopterLandDetector::get_freefall_state()
+{
+	//TODO
+	return false;
 }
 
 bool MulticopterLandDetector::get_landed_state()
@@ -183,3 +200,5 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
 	}
 }
+
+}

src/modules/land_detector/MulticopterLandDetector.h

@@ -52,6 +52,9 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <systemlib/param/param.h>
 
+namespace landdetection
+{
+
 class MulticopterLandDetector : public LandDetector
 {
 public:
@@ -66,7 +69,7 @@ protected:
 	/**
 	* @brief Runs one iteration of the land detection algorithm
 	**/
-	virtual bool update() override;
+	virtual LandDetectionResult update() override;
 
 	/**
 	* @brief Initializes the land detection algorithm
@@ -83,6 +86,11 @@ protected:
 	**/
 	bool get_landed_state();
 
+	/**
+	* @brief returns true if multicopter is in free-fall state
+	**/
+	bool get_freefall_state();
+
 private:
 
 	/**
@@ -120,4 +128,6 @@ private:
 	uint64_t _landTimer;
 };
 
+}
+
 #endif //__MULTICOPTER_LAND_DETECTOR_H__

src/modules/land_detector/VtolLandDetector.cpp

@@ -41,6 +41,9 @@
 #include "VtolLandDetector.h"
 #include <drivers/drv_hrt.h>
 
+namespace landdetection
+{
+
 VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
 	_paramHandle(),
 	_params(),
@@ -69,7 +72,7 @@ void VtolLandDetector::updateSubscriptions()
 	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
-bool VtolLandDetector::update()
+LandDetectionResult VtolLandDetector::update()
 {
 	updateSubscriptions();
 	updateParameterCache(false);
@@ -94,7 +97,9 @@ bool VtolLandDetector::update()
 
 	_was_in_air = !landed;
 
-	return landed;
+	_state = (landed) ? LANDDETECTION_RES_LANDED : LANDDETECTION_RES_FLYING;
+
+	return _state;
 }
 
 void VtolLandDetector::updateParameterCache(const bool force)
@@ -114,3 +119,5 @@ void VtolLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
 	}
 }
+
+}

src/modules/land_detector/VtolLandDetector.h

@@ -44,6 +44,9 @@
 #include "MulticopterLandDetector.h"
 #include <uORB/topics/airspeed.h>
 
+namespace landdetection
+{
+
 class VtolLandDetector : public MulticopterLandDetector
 {
 public:
@@ -59,7 +62,7 @@ private:
 	/**
 	* @brief Runs one iteration of the land detection algorithm
 	**/
-	bool update() override;
+	LandDetectionResult update() override;
 
 	/**
 	* @brief Initializes the land detection algorithm
@@ -92,3 +95,5 @@ private:
 };
 
 #endif
+
+}

src/modules/land_detector/land_detector_main.cpp

@@ -55,6 +55,9 @@
 #include "MulticopterLandDetector.h"
 #include "VtolLandDetector.h"
 
+namespace landdetection
+{
+
 //Function prototypes
 static int land_detector_start(const char *mode);
 static void land_detector_stop();
@@ -208,3 +211,5 @@ exiterr:
 	PX4_WARN("mode can either be 'fixedwing' or 'multicopter'");
 	return 1;
 }
+
+}