Revert "move ekf2 yaw param to gnss yaml, fixes"

This reverts commit 74b1f1d.
dirksavage88 · Oct 30, 2024 · e2bcd53 · e2bcd53
1 parent 74b1f1d
commit e2bcd53
Show file tree

Hide file tree

Showing 5 changed files with 21 additions and 17 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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