From 6326433c473a97f22d9ec2928d12075ad00e83dc Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 27 Jan 2016 19:04:31 +1100
Subject: [PATCH] EKF: Eliminate call to hrt_absolute_time()

This will make the library more portable
---
 EKF/estimator_base.cpp |  6 +++---
 EKF/gps_checks.cpp     | 10 +++++-----
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp
index b35e01546b..c0b3f9483f 100644
--- a/EKF/estimator_base.cpp
+++ b/EKF/estimator_base.cpp
@@ -171,7 +171,7 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
 		_time_last_gps = time_usec;
 
-		_last_valid_gps_time_us = hrt_absolute_time();
+        _last_valid_gps_time_us = _time_last_imu;
 
 		gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
 
@@ -317,7 +317,7 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps)
 		map_projection_init(&_posRef, lat, lon);
 		_gps_alt_ref = gps->alt / 1e3f;
 		_gps_initialised = true;
-		_last_gps_origin_time_us = hrt_absolute_time();
+        _last_gps_origin_time_us = _time_last_imu;
 	}
 }
 
@@ -325,7 +325,7 @@ bool EstimatorBase::position_is_valid()
 {
 	// return true if the position estimate is valid
 	// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
-	return _gps_initialised &&  (hrt_absolute_time() - _last_valid_gps_time_us) < 5e6;
+    return _gps_initialised &&  (_time_last_imu - _last_valid_gps_time_us) < 5e6;
 }
 
 void EstimatorBase::printStoredIMU()
diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp
index cefa03ea60..1464ed6f35 100644
--- a/EKF/gps_checks.cpp
+++ b/EKF/gps_checks.cpp
@@ -90,7 +90,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
 
     // Calculate time lapsesd since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
     const float filtTimeConst = 10.0f;
-    float dt = fminf(fmaxf(float(hrt_absolute_time() - _last_gps_origin_time_us)*1e-6f,0.001f),filtTimeConst);
+    float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filtTimeConst);
     float filterCoef = dt/filtTimeConst;
 
     // Calculate the horizontal drift velocity components and limit to 10x the threshold
@@ -113,7 +113,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
 
     // Save current position as the reference for next time
     map_projection_init(&_posRef, lat, lon);
-    _last_gps_origin_time_us = hrt_absolute_time();
+    _last_gps_origin_time_us = _time_last_imu;
 
     // Calculate the vertical drift velocity and limit to 10x the threshold
     velLimit = 10.0f * _params.requiredVdrift;
@@ -155,7 +155,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
 
     // assume failed first time through
     if (_lastGpsFail_us == 0) {
-        _lastGpsFail_us = hrt_absolute_time();
+        _lastGpsFail_us = _time_last_imu;
     }
 
     // if any user selected checks have failed, record the fail time
@@ -170,11 +170,11 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
     (_gpsCheckFailStatus.flags.hspeed  && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
     (_gpsCheckFailStatus.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD))
     ) {
-        _lastGpsFail_us = hrt_absolute_time();
+        _lastGpsFail_us = _time_last_imu;
     }
 
     // continuous period without fail of 10 seconds required to return a healthy status
-    if (hrt_absolute_time() - _lastGpsFail_us > 1e7) {
+    if (_time_last_imu - _lastGpsFail_us > 1e7) {
         return true;
     }
     return false;