From e2bcd538c52d257fe0dba7a2c8f52758503d49bb Mon Sep 17 00:00:00 2001
From: dirksavage88 <dirksavage88@gmail.com>
Date: Wed, 30 Oct 2024 15:54:51 -0400
Subject: [PATCH] Revert "move ekf2 yaw param to gnss yaml, fixes"

This reverts commit 74b1f1d5113959e647304df6d22b561117381aaa.
---
 src/modules/ekf2/EKF/common.h     |  1 +
 src/modules/ekf2/EKF2.cpp         | 17 ++++++++++-------
 src/modules/ekf2/EKF2.hpp         |  2 +-
 src/modules/ekf2/module.yaml      |  9 +++++++++
 src/modules/ekf2/params_gnss.yaml |  9 ---------
 5 files changed, 21 insertions(+), 17 deletions(-)

diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index 1589047e818d..a16f47b0062c 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -347,6 +347,7 @@ struct parameters {
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	// GNSS heading fusion
 	float gnss_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
+	float gps_yaw_off{0.0}; 		 ///> gps yaw offset for dual antenna gps
 # endif // CONFIG_EKF2_GNSS_YAW
 
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index c502f132d0a0..7dbee8886a52 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -97,6 +97,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_req_hdrift(_params->req_hdrift),
 	_param_ekf2_req_vdrift(_params->req_vdrift),
 	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default),
+	_param_ekf2_gps_yaw_off(_params->gps_yaw_off),
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_BAROMETER)
 	_param_ekf2_baro_ctrl(_params->baro_ctrl),
@@ -2411,14 +2412,16 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			return; //TODO: change and set to NAN
 		}
 
-		if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) {
-			if (PX4_ISFINITE(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) {
-				// Convert the offset to radians & appy offset
-				float raw_yaw_offset = matrix::wrap_pi(_param_ekf2_gps_yaw_off.get());
-				vehicle_gps_position.heading_offset = raw_yaw_offset;
-				vehicle_gps_position.heading -= raw_yaw_offset;
-			}
+#if defined(CONFIG_EKF2_GNSS_YAW)
+
+		if (std::isnan(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) {
+			// Convert the offset to radians & appy offset
+			float raw_yaw_offset = matrix::wrap_pi(math::radians(_params->gps_yaw_off));
+			vehicle_gps_position.heading_offset = raw_yaw_offset;
+			vehicle_gps_position.heading -= raw_yaw_offset;
 		}
+
+#endif //CONFIG_EKF2_GNSS_YAW
 		const float altitude_amsl = static_cast<float>(vehicle_gps_position.altitude_msl_m);
 		const float altitude_ellipsoid = static_cast<float>(vehicle_gps_position.altitude_ellipsoid_m);
 
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 0bec478e0d73..9a3b29f92ca2 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -537,7 +537,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 
 		// Used by EKF-GSF experimental yaw estimator
 		(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas_default,
-		(ParamFloat<px4::params::EKF2_GPS_YAW_OFF>) _param_ekf2_gps_yaw_off,
+		(ParamExtFloat<px4::params::EKF2_GPS_YAW_OFF>) _param_ekf2_gps_yaw_off,
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_BAROMETER)
diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml
index d8295fd60c25..f892bf38114d 100644
--- a/src/modules/ekf2/module.yaml
+++ b/src/modules/ekf2/module.yaml
@@ -111,6 +111,15 @@ parameters:
       default: 7
       min: 0
       max: 7
+    EKF2_GPS_YAW_OFF:
+      description:
+        short: Heading/Yaw offset for dual antenna GPS
+      type: float
+      default: 0.0
+      min: 0.0
+      max: 360.0
+      unit: deg
+      decimal: 3
     EKF2_GYR_NOISE:
       description:
         short: Rate gyro noise for covariance prediction
diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml
index 50ab5f1e71ae..55c530a125c9 100644
--- a/src/modules/ekf2/params_gnss.yaml
+++ b/src/modules/ekf2/params_gnss.yaml
@@ -46,15 +46,6 @@ parameters:
       min: 1.0
       unit: SD
       decimal: 1
-    EKF2_GPS_YAW_OFF:
-      description:
-        short: Heading/Yaw offset for dual antenna GPS
-      type: float
-      default: 0.0
-      min: 0.0
-      max: 360.0
-      unit: deg
-      decimal: 3
     EKF2_GPS_V_GATE:
       description:
         short: Gate size for GNSS velocity fusion