land_detector: use hover thrust if it was valid recently

- the hover thrust estimate will often invalidate upon ground contact, but before the land detector ground_contact triggers
 - use same time throughout call to avoid subtle timing surprises

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -108,9 +108,8 @@ void MulticopterLandDetector::_update_topics()
 		if (_hover_thrust_estimate_sub.update(&hte)) {
 			if (hte.valid) {
 				_params.hoverThrottle = hte.hover_thrust;
+				_hover_thrust_estimate_last_valid = hte.timestamp;
 			}
-
-			_hover_thrust_estimate_valid = hte.valid;
 		}
 	}
 }
@@ -153,7 +152,10 @@ bool MulticopterLandDetector::_get_freefall_state()
 
 bool MulticopterLandDetector::_get_ground_contact_state()
 {
-	const bool lpos_available = (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s);
+	const hrt_abstime time_now_us = hrt_absolute_time();
+
+	const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s);
+	const bool hover_thrust_estimate_valid = ((time_now_us - _hover_thrust_estimate_last_valid) < 1_s);
 
 	// land speed threshold, 90% of MPC_LAND_SPEED
 	const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
@@ -166,7 +168,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		// an accurate in-air indication.
 		float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());
 
-		if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
+		if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
 			// Widen acceptance thresholds for landed state right after arming
 			// so that motor spool-up and other effects do not trigger false negatives.
 			max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
@@ -187,7 +189,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 
 
 	// low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available
-	const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f;
+	const float thr_pct_hover = hover_thrust_estimate_valid ? 0.6f : 0.3f;
 	const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover;
 	bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle);
 
@@ -234,6 +236,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 		return true;
 	}
 
+	const hrt_abstime time_now_us = hrt_absolute_time();
 
 	// minimal throttle: initially 10% of throttle range between min and hover
 	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
@@ -246,7 +249,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	// Check if thrust output is less than the minimum throttle.
 	if (_actuator_controls_throttle <= sys_min_throttle) {
 		if (_min_thrust_start == 0) {
-			_min_thrust_start = hrt_absolute_time();
+			_min_thrust_start = time_now_us;
 		}
 
 	} else {
@@ -263,7 +266,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	float landThresholdFactor = 1.f;
 
 	// Widen acceptance thresholds for landed state right after landed
-	if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
+	if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
 		landThresholdFactor = 2.5f;
 	}
 
@@ -278,12 +281,12 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	}
 
 	// If vertical velocity is available: ground contact, no thrust, no movement -> landed
-	if ((hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
+	if (((time_now_us - _vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
 		return _ground_contact_hysteresis.get_state();
 	}
 
 	// Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds
-	return (_min_thrust_start > 0) && (hrt_elapsed_time(&_min_thrust_start) > 8_s);
+	return (_min_thrust_start > 0) && ((time_now_us - _min_thrust_start) > 8_s);
 }
 
 bool MulticopterLandDetector::_get_landed_state()

src/modules/land_detector/MulticopterLandDetector.h

@@ -112,9 +112,10 @@ private:
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 
+	hrt_abstime _hover_thrust_estimate_last_valid{0};
+
 	bool _flag_control_climb_rate_enabled{false};
 	bool _hover_thrust_initialized{false};
-	bool _hover_thrust_estimate_valid{false};
 
 	float _actuator_controls_throttle{0.f};