From c1fc893ccad5f771edd495dcbec98a099bc0549b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 2 May 2024 14:22:16 -0400
Subject: [PATCH] ekf2: move gyro/accel/wind covariance reset helpers to
 covariance.cpp

---
 src/modules/ekf2/EKF/covariance.cpp | 26 ++++++++++++++++++++++++--
 src/modules/ekf2/EKF/ekf_helper.cpp | 19 -------------------
 2 files changed, 24 insertions(+), 21 deletions(-)

diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp
index 055feef795..5190ad1213 100644
--- a/src/modules/ekf2/EKF/covariance.cpp
+++ b/src/modules/ekf2/EKF/covariance.cpp
@@ -282,6 +282,25 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
 	P.uncorrelateCovarianceSetVariance<State::quat_nominal.dof>(State::quat_nominal.idx, rot_var_ned);
 }
 
+void Ekf::resetGyroBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
+}
+
+void Ekf::resetGyroBiasZCov()
+{
+	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
+}
+
+void Ekf::resetAccelBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
+}
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void Ekf::resetMagCov()
 {
@@ -295,7 +314,10 @@ void Ekf::resetMagCov()
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-void Ekf::resetGyroBiasZCov()
+#if defined(CONFIG_EKF2_WIND)
+void Ekf::resetWindCov()
 {
-	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
 }
+#endif // CONFIG_EKF2_WIND
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index b3c036445c..483da288a2 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -302,13 +302,6 @@ void Ekf::resetGyroBias()
 	resetGyroBiasCov();
 }
 
-void Ekf::resetGyroBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
-}
-
 void Ekf::resetAccelBias()
 {
 	// Zero the accel bias states
@@ -317,13 +310,6 @@ void Ekf::resetAccelBias()
 	resetAccelBiasCov();
 }
 
-void Ekf::resetAccelBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
-}
-
 // get EKF innovation consistency check status information comprising of:
 // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
 // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
@@ -675,11 +661,6 @@ void Ekf::resetWindToZero()
 	resetWindCov();
 }
 
-void Ekf::resetWindCov()
-{
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
-}
 #endif // CONFIG_EKF2_WIND
 
 void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)