From dacd0b64d4db6771239f78c26fd632e081c9eced Mon Sep 17 00:00:00 2001
From: MarvinHarms <harmsm@student.ethz.ch>
Date: Tue, 23 Aug 2022 08:34:31 +0200
Subject: [PATCH] transfer push (not tested)

---
 Tools/sitl_gazebo                             |  2 +-
 .../FixedwingPositionINDIControl.cpp          |  4 +-
 .../FixedwingShearEstimator.cpp               | 63 ++++++++++++++++++-
 .../FixedwingShearEstimator.hpp               |  9 +++
 4 files changed, 72 insertions(+), 6 deletions(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index d3cb480a15..a3eef9d3a1 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit d3cb480a15d64a153513cd9cb68abd8bb5f4c1f8
+Subproject commit a3eef9d3a17078896f40abb0deaabfb0faad8c65
diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
index 9cd2687bbe..1cc6f7178b 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
@@ -463,8 +463,8 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
     trajec_<type>_<V_max>_<alpha>_<energy>_
     with
     <type> in {nominal, robust}
-    <V_max> in [08,24]
-    <alpha> in [0.10, 1.00]
+    <V_max> in [08,12]
+    <alpha> in [0.20, 1.00]
     <energy> in [E_min, E_max]
     */
 
diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
index 6a5a20058f..4acf08f0a3 100644
--- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
@@ -323,9 +323,6 @@ FixedwingShearEstimator::Run()
         // get current measurement
         _current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered)/_unit_v;
         _current_height = Vector3f(soaring_controller_wind.position)(2)/_unit_h;
-        //_current_wind(0) = 0.f;
-        //_current_wind(1) = -(10.f/_unit_v)/(1+expf(-1.f/_unit_a*(_current_height- 110.f/_unit_h)));
-        //_current_wind(2) = 0.f;
 
         if (true) {
             // prior update
@@ -343,6 +340,12 @@ FixedwingShearEstimator::Run()
 
         }
 
+        // get the correct shear params for trajectory selection
+        float v = _findClosest(_v_max_arr, 5, _shear_v_max);    // wind velocity
+        float a = _findClosest(_alpha_arr, 9, _shear_alpha);    // shear strength
+        float heading = -atan2f(_X_posterior_horizontal(1)-_X_posterior_horizontal(3),
+                                _X_posterior_horizontal(0)-_X_posterior_horizontal(2));
+
         // publish shear params
         // ========================================
         // publish controller position in ENU frame
@@ -404,6 +407,60 @@ bool FixedwingShearEstimator::check_feasibility()
     return true;
 }
 
+float
+FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target)
+{
+    if (target - val1 >= val2 - target)
+        return val2;
+    else
+        return val1;
+}
+
+float
+FixedwingPositionINDIControl::_findClosest(float arr[], int n, float target)
+{
+    // Corner cases
+    if (target <= arr[0])
+        return arr[0];
+    if (target >= arr[n - 1])
+        return arr[n - 1];
+ 
+    // Doing binary search
+    int i = 0, j = n, mid = 0;
+    while (i < j) {
+        mid = (i + j) / 2;
+ 
+        if ((float)fabs(arr[mid]-target) < (float)0.0001f)
+            return arr[mid];
+ 
+        /* If target is less than array element,
+            then search in left */
+        if (target < arr[mid]) {
+ 
+            // If target is greater than previous
+            // to mid, return closest of two
+            if (mid > 0 && target > arr[mid - 1])
+                return _getClosest(arr[mid - 1],
+                                  arr[mid], target);
+ 
+            /* Repeat for left half */
+            j = mid;
+        }
+ 
+        // If target is greater than mid
+        else {
+            if (mid < n - 1 && target < arr[mid + 1])
+                return _getClosest(arr[mid],
+                                  arr[mid + 1], target);
+            // update i
+            i = mid + 1;
+        }
+    }
+ 
+    // Only single element left after search
+    return arr[mid];
+}
+
 
 int FixedwingShearEstimator::task_spawn(int argc, char *argv[])
 {
diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp
index 406d036c0d..4b4e267f63 100644
--- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp
+++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp
@@ -99,6 +99,10 @@ private:
     void    publish_estimate();
     void	status_publish();
 
+    // helper methods
+	float _getClosest(float val1, float val2, float taget);	// get float closest to target
+	float _findClosest(float arr[], int n, float target);	// return element in arr closest to n
+
     // control variables
     hrt_abstime _last_run{0};
     uint _reset_counter = {};
@@ -137,6 +141,11 @@ private:
     // helper variables
     float _init_height = {};
 
+    // trajectory selection variables
+    // vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength	
+	float _v_max_arr[5] = {8.f,9.f,10.f,11.f,12.f};	
+	float _alpha_arr[9] = {0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f};	
+
 };