transfer push (not tested)

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]
     */
 

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[])
 {

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};	
+
 };