From eb7167fd11913856185c21e55182a4611edf38a4 Mon Sep 17 00:00:00 2001
From: JaeyoungLim <jalim@ethz.ch>
Date: Wed, 15 Nov 2023 10:26:31 +0100
Subject: [PATCH] Save shear params

---
 .../FixedwingPositionINDIControl.cpp                 |  4 ++++
 .../FixedwingPositionINDIControl.hpp                 | 12 ++++++------
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
index 46b55e6743..d09f6fbeba 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
@@ -433,6 +433,10 @@ FixedwingPositionINDIControl::soaring_estimator_shear_poll()
 			_soaring_feasible =  _soaring_estimator_shear.soaring_feasible;
 			// the initial speed of the target trajectory can safely be updated during soaring :)
 			_shear_aspd = _soaring_estimator_shear.aspd;
+			_param_shear_estimated_v_max.set(_shear_v_max);
+			_param_shear_estimated_alpha.set(_shear_alpha);
+			_param_shear_estimated_h_ref.set(_shear_h_ref);
+			_param_shear_estimated_heading.set(_shear_heading);
 		}
 
 	}
diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
index 014d53b413..3066ba9946 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
@@ -230,11 +230,11 @@ private:
 		(ParamInt<px4::params::DS_SWITCH_CLOOP>) _param_switch_cloop,
 		// manual switch if we are in SITL mode
 		(ParamInt<px4::params::DS_SWITCH_SITL>) _param_switch_sitl,
-		(ParamInt<px4::params::DS_SWITCH_SITL>) _shear_estimated_v_max,
-		(ParamInt<px4::params::DS_WINDEST_HEIGHT>) _shear_estimated_alpha,
-		(ParamInt<px4::params::DS_WINDEST_HDG>) _shear_estimated_h_ref,
-		(ParamInt<px4::params::DS_WINDEST_ALPHA>) _shear_estimated_heading,
-		(ParamInt<px4::params::DS_WINDEST_VMAX>) _shear_estimated_aspd
+		(ParamInt<px4::params::DS_SWITCH_SITL>) _param_shear_estimated_v_max,
+		(ParamInt<px4::params::DS_WINDEST_HEIGHT>) _param_shear_estimated_alpha,
+		(ParamInt<px4::params::DS_WINDEST_HDG>) _param_shear_estimated_h_ref,
+		(ParamInt<px4::params::DS_WINDEST_ALPHA>) _param_shear_estimated_heading,
+		(ParamInt<px4::params::DS_WINDEST_VMAX>) _param_shear_estimated_aspd
 
 	)
 
@@ -449,4 +449,4 @@ private:
 
 
 
-#endif // FIXEDWINGPOSITIONINDICONTROL_HPP_
\ No newline at end of file
+#endif // FIXEDWINGPOSITIONINDICONTROL_HPP_