From 36fb465c5a2eac864aaa6d2313323f4a73eda9c4 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 27 Jan 2022 15:48:01 +0100
Subject: [PATCH] fix unit tests

---
 src/modules/ekf2/EKF/terrain_estimator.cpp            |  1 +
 src/modules/ekf2/EKF2.cpp                             |  2 +-
 src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp | 10 +++++-----
 src/modules/ekf2/test/test_EKF_initialization.cpp     |  2 +-
 4 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp
index 882d8906e5..2b0e5fb0a8 100644
--- a/src/modules/ekf2/EKF/terrain_estimator.cpp
+++ b/src/modules/ekf2/EKF/terrain_estimator.cpp
@@ -366,4 +366,5 @@ void Ekf::fuseHaglAllStates()
 			_innov_check_fail_status.flags.reject_ver_pos = true;
 		}
 	}
+	updateTerrainValidity();
 }
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index a32e7b0c5e..bded1e8e40 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -1017,7 +1017,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 	static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
 
 	// Get covariances to vehicle odometry
-	float covariances[24];
+	float covariances[25];
 	_ekf.covariances_diagonal().copyTo(covariances);
 
 	// initially set pose covariances to 0
diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp
index 4161c00975..b667eec722 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp
@@ -21,13 +21,13 @@ void EkfLogger::writeStateToFile()
 		_file << "Timestamp";
 
 		if (_state_logging_enabled) {
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << ",state[" << i << "]";
 			}
 		}
 
 		if (_variance_logging_enabled) {
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << ",variance[" << i << "]";
 			}
 		}
@@ -52,15 +52,15 @@ void EkfLogger::writeState()
 		_file << time;
 
 		if (_state_logging_enabled) {
-			matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector();
+			matrix::Vector<float, 25> state = _ekf->getStateAtFusionHorizonAsVector();
 
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << "," << std::setprecision(3) << state(i);
 			}
 		}
 
 		if (_variance_logging_enabled) {
-			matrix::Vector<float, 24> variance = _ekf->covariances_diagonal();
+			matrix::Vector<float, 25> variance = _ekf->covariances_diagonal();
 
 			for (int i = 0; i < 24; i++) {
 				_file << "," << variance(i);
diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp
index 967f4f6590..1dba852f1c 100644
--- a/src/modules/ekf2/test/test_EKF_initialization.cpp
+++ b/src/modules/ekf2/test/test_EKF_initialization.cpp
@@ -169,7 +169,7 @@ TEST_F(EkfInitializationTest, gyroBias)
 		if (fabsf(accel_bias(2)) > 0.3f) {
 
 			// Print state covariance and correlation matrices for debugging
-			const matrix::SquareMatrix<float, 24> P = _ekf->covariances();
+			const matrix::SquareMatrix<float, 25> P = _ekf->covariances();
 
 			printf("State covariance:\n");