From caecdbd60bc93240fbb3bce1513bbce71d86fb29 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 18 Apr 2017 16:50:48 +0200
Subject: [PATCH] land_detector: treat altitude mode like position mode with
vertical checks but without horizontal checks because in altitude mode we
have a baro available and can therefore check vertical movement we can not
check horizontal movement but I consider the checks for landing still pretty
safe unlike in manual mode we are not allowed to disarm before land detection
in altitude mode
---
.../land_detector/MulticopterLandDetector.cpp | 19 ++++++++++++-------
.../land_detector/MulticopterLandDetector.h | 1 +
2 files changed, 13 insertions(+), 7 deletions(-)
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index f197f6934b..c3ec4e64b9 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -187,7 +187,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
if (manual_control_idle_or_auto && _has_minimal_thrust() &&
- (!verticalMovement || !_has_position_lock())) {
+ (!verticalMovement || !_has_altitude_lock())) {
return true;
}
@@ -226,7 +226,7 @@ bool MulticopterLandDetector::_get_landed_state()
}
// Return status based on armed state and throttle if no position lock is available.
- if (!_has_position_lock()) {
+ if (!_has_altitude_lock()) {
// The system has minimum trust set (manual or in failsafe)
// if this persists for 8 seconds AND the drone is not
// falling consider it to be landed. This should even sustain
@@ -260,7 +260,8 @@ bool MulticopterLandDetector::_get_landed_state()
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
- if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating && !horizontalMovement) {
+ if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating &&
+ (!horizontalMovement || !_has_position_lock())) {
// Ground contact, no thrust and no movement -> landed
return true;
}
@@ -308,12 +309,16 @@ float MulticopterLandDetector::_get_max_altitude()
return valid_altitude_max;
}
+bool MulticopterLandDetector::_has_altitude_lock()
+{
+ return _vehicleLocalPosition.timestamp != 0 &&
+ hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500000 &&
+ _vehicleLocalPosition.z_valid;
+}
+
bool MulticopterLandDetector::_has_position_lock()
{
- return !(_vehicleLocalPosition.timestamp == 0 ||
- hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
- !_vehicleLocalPosition.xy_valid ||
- !_vehicleLocalPosition.z_valid);
+ return _has_altitude_lock() && _vehicleLocalPosition.xy_valid;
}
bool MulticopterLandDetector::_has_manual_control_present()
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
index 6b1614ef2a..2579280cc7 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -135,6 +135,7 @@ private:
/* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */
float _get_takeoff_throttle();
+ bool _has_altitude_lock();
bool _has_position_lock();
bool _has_manual_control_present();
bool _has_minimal_thrust();
--
GitLab