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

landdetector: add additional landdetection state

Browse Source
This commit is contained in:
Dennis Mannhart
2017-08-01 19:31:34 +02:00
committed by Lorenz Meier
parent 529def11e8
commit f8e9f380d0
9 changed files with 76 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.cpp
+10
View File
@@ -57,6 +57,7 @@ LandDetector::LandDetector() :
_state{},
_freefall_hysteresis(false),
_landed_hysteresis(true),
_maybe_landed_hysteresis(true),
_ground_contact_hysteresis(true),
_total_flight_time{0},
_takeoff_time{0},
@@ -64,6 +65,7 @@ LandDetector::LandDetector() :
{
// 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);
_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US);
_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
}
@@ -93,6 +95,7 @@ void LandDetector::_cycle()
_landDetected.freefall = false;
_landDetected.landed = false;
_landDetected.ground_contact = false;
_landDetected.maybe_landed = false;
_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
@@ -117,6 +120,7 @@ void LandDetector::_cycle()
bool freefallDetected = (_state == LandDetectionState::FREEFALL);
bool landDetected = (_state == LandDetectionState::LANDED);
bool maybe_landedDetected = (_state == LandDetectionState::MAYBE_LANDED);
bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
// Only publish very first time or when the result has changed.
@@ -124,6 +128,7 @@ void LandDetector::_cycle()
(_landDetected.freefall != freefallDetected) ||
(_landDetected.landed != landDetected) ||
(_landDetected.ground_contact != ground_contactDetected) ||
(_landDetected.maybe_landed != maybe_landedDetected) ||
(fabsf(_landDetected.alt_max - alt_max_prev) > FLT_EPSILON)) {
if (!landDetected && _landDetected.landed) {
@@ -144,6 +149,7 @@ void LandDetector::_cycle()
_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
_landDetected.landed = (_state == LandDetectionState::LANDED);
_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
_landDetected.maybe_landed = (_state == LandDetectionState::MAYBE_LANDED);
_landDetected.alt_max = _altitude_max;
int instance;
@@ -188,6 +194,7 @@ void LandDetector::_update_state()
* with higher priority for landed */
_freefall_hysteresis.set_state_and_update(_get_freefall_state());
_landed_hysteresis.set_state_and_update(_get_landed_state());
_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state());
_ground_contact_hysteresis.set_state_and_update(_landed_hysteresis.get_state() || _get_ground_contact_state());
if (_freefall_hysteresis.get_state()) {
@@ -196,6 +203,9 @@ void LandDetector::_update_state()
} else if (_landed_hysteresis.get_state()) {
_state = LandDetectionState::LANDED;
} else if (_maybe_landed_hysteresis.get_state()) {
_state = LandDetectionState::MAYBE_LANDED;
} else if (_ground_contact_hysteresis.get_state()) {
_state = LandDetectionState::GROUND_CONTACT;