From 34ffffa02146eccd6e1f8dd7dd6ca08d3dae752d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 8 Jun 2016 11:56:07 +1000
Subject: [PATCH] EKF: Prevent use of non time-stamped invalid data during
 initialisation

Fixes bad height initialisation seen intermittently with snapdragon
---
 EKF/ekf.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index b4f09b02fb..81b442f216 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -399,10 +399,10 @@ bool Ekf::initialiseFilter(void)
 
 	// Sum the magnetometer measurements
 	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		if (_mag_counter == 0 && _mag_sample_delayed.time_us !=0) {
+		if ((_mag_counter == 0) && (_mag_sample_delayed.time_us != 0)) {
 			// initialise the counter when we start getting data from the buffer
 			_mag_counter = 1;
-		} else if (_mag_counter != 0) {
+		} else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
 			// increment the sample count and apply a LPF to the measurement
 			_mag_counter ++;
 			// don't start using data until we can be certain all bad initial data has been flushed
@@ -418,7 +418,7 @@ bool Ekf::initialiseFilter(void)
 
 	// Count the number of external vision measurements received
 	if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
-		if (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) {
+		if ((_ev_counter == 0) && (_ev_sample_delayed.time_us != 0)) {
 			// initialise the counter
 			_ev_counter = 1;
 			// set the height fusion mode to use external vision data when we start getting valid data from the buffer
@@ -428,7 +428,7 @@ bool Ekf::initialiseFilter(void)
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = true;
 			}
-		} else if (_ev_counter != 0) {
+		} else if ((_ev_counter != 0) && (_ev_sample_delayed.time_us != 0)) {
 			// increment the sample count
 			_ev_counter ++;
 		}
@@ -442,14 +442,14 @@ bool Ekf::initialiseFilter(void)
 	// accumulate enough height measurements to be confident in the qulaity of the data
 	if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
 		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
-			if (_hgt_counter == 0 && _range_sample_delayed.time_us != 0) {
+			if ((_hgt_counter == 0) && (_range_sample_delayed.time_us != 0)) {
 				// initialise the counter height fusion method when we start getting data from the buffer
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = true;
 				_control_status.flags.ev_hgt = false;
 				_hgt_counter = 1;
-			} else if (_hgt_counter != 0) {
+			} else if ((_hgt_counter != 0) && (_range_sample_delayed.time_us != 0)) {
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
 				// don't start using data until we can be certain all bad initial data has been flushed
@@ -467,13 +467,13 @@ bool Ekf::initialiseFilter(void)
 		// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
 		// later when it passes checks.
 		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-			if (_hgt_counter == 0 && _baro_sample_delayed.time_us != 0) {
+			if ((_hgt_counter == 0) && (_baro_sample_delayed.time_us != 0)) {
 				// initialise the counter and height fusion method when we start getting data from the buffer
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
 				_hgt_counter = 1;
-			} else if (_hgt_counter != 0) {
+			} else if ((_hgt_counter != 0) && (_baro_sample_delayed.time_us != 0)) {
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
 				// don't start using data until we can be certain all bad initial data has been flushed