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

LandDetector:

Browse Source 
- constructor initalization fix
- set trigger time for ground contact hysteresis
- updated ground_contact_state logic
MulticopterLandDetector:
- added hysteresis for ground_contact
VtolLandDetector:
- get_ground_contact_state function that return the one form MultcopterLandDetector
FixedWingLandDetector:
- get_ground_contact_state with a return false: requires implementation
This commit is contained in:
Dennis Mannhart
2017-01-28 16:23:04 +01:00
committed by Lorenz Meier
parent f961a12d9a
commit 2f164602b4
7 changed files with 39 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.cpp
+22 -3
View File
@@ -52,14 +52,18 @@ namespace land_detector
LandDetector::LandDetector() :
_landDetectedPub(nullptr),
_landDetected{0, false, false},
_parameterSub(0),
_state{},
_freefall_hysteresis(false),
_landed_hysteresis(true),
_ground_contact_hysteresis(false),
_taskShouldExit(false),
_taskIsRunning(false),
_work{}
{
// Use Trigger time when transitioning from in-air (false) to landed (true).
// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
}
LandDetector::~LandDetector()
@@ -98,6 +102,7 @@ void LandDetector::_cycle()
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = false;
_landDetected.freefall = false;
_landDetected.ground_contact = false;
// Initialize uORB topics.
_initialize_topics();
@@ -116,15 +121,18 @@ void LandDetector::_cycle()
bool landDetected = (_state == LandDetectionState::LANDED);
bool freefallDetected = (_state == LandDetectionState::FREEFALL);
bool groundContactDetected = ( _state == LandDetectionState::GROUND_CONTACT);
// Only publish very first time or when the result has changed.
if ((_landDetectedPub == nullptr) ||
(_landDetected.landed != landDetected) ||
(_landDetected.freefall != freefallDetected)) {
(_landDetected.freefall != freefallDetected) ||
(_landDetected.ground_contact != groundContactDetected)) {
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = (_state == LandDetectionState::LANDED);
_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
int instance;
orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
@@ -160,15 +168,26 @@ void LandDetector::_check_params(const bool force)
void LandDetector::_update_state()
{
/* ground contact and landed can be true simultaneously but only one state can be true at a particular time
* with higher priority for landed */
bool groundContact = _get_ground_contact_state();
bool landed = _get_landed_state();
bool freefall = _get_freefall_state();
_ground_contact_hysteresis.set_state_and_update(groundContact);
_landed_hysteresis.set_state_and_update(landed);
_freefall_hysteresis.set_state_and_update(_get_freefall_state());
_freefall_hysteresis.set_state_and_update(freefall);
if (_freefall_hysteresis.get_state()) {
_state = LandDetectionState::FREEFALL;
} else if (_landed_hysteresis.get_state()) {
_state = LandDetectionState::LANDED;
/* need to set ground_contact_state to false */
_ground_contact_hysteresis.set_state_and_update(false);
} else if (_ground_contact_hysteresis.get_state()){
_state = LandDetectionState::GROUND_CONTACT;
} else {
_state = LandDetectionState::FLYING;