From 635ceccfdb9cf6bdd56ccfed5acb66d947959d50 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 20 Jan 2020 17:14:00 -0500
Subject: [PATCH] land_detector: relax data timeouts uniformly

 * if we're hitting these timeouts there are much larger problems in the system
---
 .../land_detector/FixedwingLandDetector.cpp     |  2 +-
 .../land_detector/MulticopterLandDetector.cpp   | 17 +++--------------
 src/modules/land_detector/VtolLandDetector.cpp  |  2 +-
 3 files changed, 5 insertions(+), 16 deletions(-)

diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index c7ce8028b3..37bc811cf9 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -66,7 +66,7 @@ bool FixedwingLandDetector::_get_landed_state()
 
 	bool landDetected = false;
 
-	if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms) {
+	if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) {
 
 		// Horizontal velocity complimentary filter.
 		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index e682753aab..79176247b1 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -147,7 +147,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get()  * 2.5f;
 
 	} else {
-
 		// Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate
 		float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
 				       _param_lndmc_z_vel_max.get();
@@ -171,9 +170,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 
 bool MulticopterLandDetector::_get_maybe_landed_state()
 {
-	// Time base for this function
-	const hrt_abstime now = hrt_absolute_time();
-
 	// When not armed, consider to be maybe-landed
 	if (!_actuator_armed.armed) {
 		return true;
@@ -181,7 +177,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 
 	if (_has_minimal_thrust()) {
 		if (_min_trust_start == 0) {
-			_min_trust_start = now;
+			_min_trust_start = hrt_absolute_time();
 		}
 
 	} else {
@@ -224,19 +220,15 @@ bool MulticopterLandDetector::_get_landed_state()
 
 	// reset the landed_time
 	if (!_maybe_landed_hysteresis.get_state()) {
-
 		_landed_time = 0;
 
 	} else if (_landed_time == 0) {
-
 		_landed_time = hrt_absolute_time();
-
 	}
 
 	// if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0)
 	// therefore check if all other condition of the landed state remain true
 	return _maybe_landed_hysteresis.get_state();
-
 }
 
 float MulticopterLandDetector::_get_max_altitude()
@@ -265,9 +257,7 @@ float MulticopterLandDetector::_get_max_altitude()
 
 bool MulticopterLandDetector::_has_altitude_lock()
 {
-	return _vehicle_local_position.timestamp != 0 &&
-	       hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms &&
-	       _vehicle_local_position.z_valid;
+	return (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.z_valid;
 }
 
 bool MulticopterLandDetector::_has_position_lock()
@@ -277,8 +267,7 @@ bool MulticopterLandDetector::_has_position_lock()
 
 bool MulticopterLandDetector::_is_climb_rate_enabled()
 {
-	bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0)
-			   && (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 500_ms);
+	bool has_updated = (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 1_s);
 
 	return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated
 		&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp
index 9dd624f7c8..a3f4292e53 100644
--- a/src/modules/land_detector/VtolLandDetector.cpp
+++ b/src/modules/land_detector/VtolLandDetector.cpp
@@ -74,7 +74,7 @@ bool VtolLandDetector::_get_landed_state()
 	bool landed = MulticopterLandDetector::_get_landed_state();
 
 	// for vtol we additionally consider airspeed
-	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000 && _airspeed.confidence > 0.99f
+	if (hrt_elapsed_time(&_airspeed.timestamp) < 1_s && _airspeed.confidence > 0.99f
 	    && PX4_ISFINITE(_airspeed.indicated_airspeed_m_s)) {
 
 		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.indicated_airspeed_m_s;