From 477a42e6f67d88b382dd535a6d5bfb0e887055cb Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 21 Mar 2018 22:59:34 +1100
Subject: [PATCH] ekf2: Enable adjustment of nav validity timeout

---
 src/modules/ekf2/ekf2_main.cpp |  5 ++++-
 src/modules/ekf2/ekf2_params.c | 11 +++++++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 7c42ed6b71..fb630bfd32 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -287,6 +287,8 @@ private:
 		(ParamExtInt<px4::params::EKF2_AID_MASK>)
 		_fusion_mode,		///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
 		(ParamExtInt<px4::params::EKF2_HGT_MODE>) _vdist_sensor_type,	///< selects the primary source for height data
+		(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
+		_valid_timeout_max,	///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
 
 		// range finder fusion
 		(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _range_noise,	///< observation noise for range finder measurements (m)
@@ -353,7 +355,7 @@ private:
 		(ParamExtFloat<px4::params::EKF2_TAU_POS>)
 		_tau_pos,		///< time constant used by the output position complementary filter (sec)
 
-		// IMU switch on bias paameters
+		// IMU switch on bias parameters
 		(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>) _gyr_bias_init,	///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
 		(ParamExtFloat<px4::params::EKF2_ABIAS_INIT>)
 		_acc_bias_init,	///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
@@ -453,6 +455,7 @@ Ekf2::Ekf2():
 	_requiredVdrift(_params->req_vdrift),
 	_fusion_mode(_params->fusion_mode),
 	_vdist_sensor_type(_params->vdist_sensor_type),
+	_valid_timeout_max(_params->valid_timeout_max),
 	_range_noise(_params->range_noise),
 	_range_noise_scaler(_params->range_noise_scaler),
 	_range_innov_gate(_params->range_innov_gate),
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 6d79045d08..e6a1c30fae 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -606,6 +606,17 @@ PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
  */
 PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0);
 
+/**
+ * Maximum lapsed time from last fusion of measurements that constrain velocity drift before the EKF will report the horizontal nav solution as invalid.
+ *
+ * @group EKF2
+ * @group EKF2
+ * @min 500000
+ * @max 10000000
+ * @unit uSec
+ */
+PARAM_DEFINE_INT32(EKF2_NOAID_TOUT, 5000000);
+
 /**
  * Measurement noise for range finder fusion
  *
-- 
GitLab