From 9d1558af251bea4313366eb2bc929933583fd2b1 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 29 Dec 2021 10:23:42 -0500
Subject: [PATCH] ekf2: selector use status timestamp for timeout rather than
 timestamp_sample (delayed time horizon)

 - update stale checks for publication to use hrt elapsed time from
timestamp sample
---
 src/modules/ekf2/EKF2Selector.cpp | 16 ++++++++--------
 src/modules/ekf2/EKF2Selector.hpp |  2 +-
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp
index 75ad802ca5..7ea1f71407 100644
--- a/src/modules/ekf2/EKF2Selector.cpp
+++ b/src/modules/ekf2/EKF2Selector.cpp
@@ -261,7 +261,7 @@ bool EKF2Selector::UpdateErrorScores()
 
 		if (_instance[i].estimator_status_sub.update(&status)) {
 
-			_instance[i].timestamp_sample_last = status.timestamp_sample;
+			_instance[i].timestamp_last = status.timestamp;
 
 			_instance[i].accel_device_id = status.accel_device_id;
 			_instance[i].gyro_device_id = status.gyro_device_id;
@@ -302,14 +302,14 @@ bool EKF2Selector::UpdateErrorScores()
 			_instance[i].timeout = false;
 
 			if (!_instance[i].warning) {
-				_instance[i].time_last_no_warning = status.timestamp_sample;
+				_instance[i].time_last_no_warning = status.timestamp;
 			}
 
 			if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) {
 				_instance[i].relative_test_ratio = 0;
 			}
 
-		} else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout)) {
+		} else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_last) > status_timeout)) {
 			_instance[i].healthy.set_state_and_update(false, hrt_absolute_time());
 			_instance[i].timeout = true;
 		}
@@ -395,7 +395,7 @@ void EKF2Selector::PublishVehicleAttitude()
 		// ensure monotonically increasing timestamp_sample through reset, don't publish
 		//  estimator's attitude for system (vehicle_attitude) if it's stale
 		if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample)
-		    || (attitude.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+		    || (hrt_elapsed_time(&attitude.timestamp) > 10_ms)) {
 
 			publish = false;
 		}
@@ -497,7 +497,7 @@ void EKF2Selector::PublishVehicleLocalPosition()
 		// ensure monotonically increasing timestamp_sample through reset, don't publish
 		//  estimator's local position for system (vehicle_local_position) if it's stale
 		if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample)
-		    || (local_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+		    || (hrt_elapsed_time(&local_position.timestamp) > 20_ms)) {
 
 			publish = false;
 		}
@@ -536,7 +536,7 @@ void EKF2Selector::PublishVehicleOdometry()
 		// ensure monotonically increasing timestamp_sample through reset, don't publish
 		//  estimator's odometry for system (vehicle_odometry) if it's stale
 		if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample)
-		    || (odometry.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+		    || (hrt_elapsed_time(&odometry.timestamp) > 20_ms)) {
 
 			publish = false;
 		}
@@ -602,7 +602,7 @@ void EKF2Selector::PublishVehicleGlobalPosition()
 		// ensure monotonically increasing timestamp_sample through reset, don't publish
 		//  estimator's global position for system (vehicle_global_position) if it's stale
 		if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample)
-		    || (global_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+		    || (hrt_elapsed_time(&global_position.timestamp) > 20_ms)) {
 
 			publish = false;
 		}
@@ -633,7 +633,7 @@ void EKF2Selector::PublishWindEstimate()
 		// ensure monotonically increasing timestamp_sample through reset, don't publish
 		//  estimator's wind for system (wind) if it's stale
 		if ((wind.timestamp_sample <= _wind_last.timestamp_sample)
-		    || (wind.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+		    || (hrt_elapsed_time(&wind.timestamp) > 100_ms)) {
 
 			publish = false;
 		}
diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp
index 628fa1a090..95dc71a2e8 100644
--- a/src/modules/ekf2/EKF2Selector.hpp
+++ b/src/modules/ekf2/EKF2Selector.hpp
@@ -120,7 +120,7 @@ private:
 		uORB::Subscription estimator_odometry_sub;
 		uORB::Subscription estimator_wind_sub;
 
-		uint64_t timestamp_sample_last{0};
+		uint64_t timestamp_last{0};
 
 		uint32_t accel_device_id{0};
 		uint32_t gyro_device_id{0};