fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added room in land_detector code for free-fall detection

Browse Source
This commit is contained in:
Emmanuel Roussel
2016-04-18 22:27:11 +02:00
committed by Roman
parent e70804c9d0
commit 35110a52f9
10 changed files with 118 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.cpp
+11 -2
View File
@@ -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;
}
}