diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg
index ce6ea8e9a149a21f04289f239c3d453376bd950f..d886d31b05f1e3bcf9a314c72940a103250d50d9 100644
--- a/msg/vehicle_land_detected.msg
+++ b/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
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 327e780a3227bfba6600ed1e099937792598ae86..de41a1aa4194062e1fc8770d63f527b5c62daef4 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/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);
}
}
+
+}
diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h
index c516788a3fbdfada4085387baf869e0300231caf..f450f94b825be4971ae28f9410e85ce4858519ee 100644
--- a/src/modules/land_detector/FixedwingLandDetector.h
+++ b/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__
diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
index 5140a2809c010f6e22f6fc212a85616f05b16c76..a8b61fb1aeb108173faf6094c708f14a50b6ba1c 100644
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/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;
}
+
+}
diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h
index 4b418f65239ab57c8dbfbc1d33ca32adcc27204e..ce7b862684abc4379a263991e505d06962028953 100644
--- a/src/modules/land_detector/LandDetector.h
+++ b/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__
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index 650ae5724d3314b338e09b2f29cbb3962273c002..03ece61eb2453a79fd2314c8682f4b9266b61671 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/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);
}
}
+
+}
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
index 4e520fea9472f6e4291c10fb70f244ae8ad75922..7eda290d8b8670b2d348bd1e9c7d42130bb9e484 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/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__
diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp
index 6c2fa9be8ad4b9ba3dc274b313a53a4878b4fcd6..50a39c5fea7b274eaff00bea6c5ace8039ea8b80 100644
--- a/src/modules/land_detector/VtolLandDetector.cpp
+++ b/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);
}
}
+
+}
diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h
index 2c942ed06fc5ba586d3f3e6794b5d238f66c7b0d..a9b409f6104d0522887ce2b7a584e54cdad56cc3 100644
--- a/src/modules/land_detector/VtolLandDetector.h
+++ b/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
+
+}
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
index 1179b10eae6afff1bd71e286db6532faf10399d9..5572c1b2f143e651f72f1778af123450c2ce34e1 100644
--- a/src/modules/land_detector/land_detector_main.cpp
+++ b/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;
}
+
+}