changed basis fun sigma back to 1.0

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -77,6 +77,9 @@ FixedwingPositionINDIControl::init()
 		return false;
 	}
     _read_trajectory_coeffs_csv("trajectory0.csv");
+    _read_trajectory_coeffs_csv("trajectory1.csv");
+    _read_trajectory_coeffs_csv("trajectory2.csv");
+    //_read_trajectory_coeffs_csv("trajectory3.csv");
 
     // initialize transformations
     _R_ned_to_enu *= 0.f;
@@ -646,7 +649,7 @@ FixedwingPositionINDIControl::_get_basis_funs(float t)
 {
     Vector<float, _num_basis_funs> vec;
     vec(0) = 1.0f;
-    float sigma = 1.0f/float(_num_basis_funs);
+    float sigma = 1.0f/_num_basis_funs;
     for(uint i=1; i<_num_basis_funs; i++){
         float fun1 = sinf(M_PI_F*t);
         float fun2 = exp(-powf((t-float(i)/float(_num_basis_funs)),2)/sigma);
@@ -1138,7 +1141,7 @@ int FixedwingPositionINDIControl::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-fw_dyn_soar_control is the fixed wing controller for soaring tasks.
+fw_dyn_soar_control is the fixed wing controller for dynamic soaring tasks.
 
 )DESCR_STR");
 

src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv

@@ -1,3 +1,3 @@
 -0.000064,-3020.233571,10609.960177,-17956.458964,15735.479961,-2399.573434,-11421.854705,12388.936542,120.944433,-12530.869640,11346.431128,2643.369342,-15999.009519,18127.094775,-10676.696033,3032.667571
 -100.005984,-4686.100637,21963.998713,-50566.542718,71908.811359,-61683.065460,15730.546677,39386.062413,-63952.599923,39525.510553,15526.730604,-61505.752706,71804.582542,-50525.803330,21954.858741,-4685.311429
-100.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+100.000000,-0.013254,-0.012083,-0.281296,0.324078,-0.438216,0.063212,0.096122,0.034719,0.060176,0.153558,-0.402818,0.689494,-0.192379,0.050574,-0.011868

src/modules/fw_dyn_soar_control/trajectories/trajectory0_sigma_1.csv

@@ -0,0 +1,3 @@
+-0.000064,-3020.233571,10609.960177,-17956.458964,15735.479961,-2399.573434,-11421.854705,12388.936542,120.944433,-12530.869640,11346.431128,2643.369342,-15999.009519,18127.094775,-10676.696033,3032.667571
+-100.005984,-4686.100637,21963.998713,-50566.542718,71908.811359,-61683.065460,15730.546677,39386.062413,-63952.599923,39525.510553,15526.730604,-61505.752706,71804.582542,-50525.803330,21954.858741,-4685.311429
+100.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000

src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv

@@ -1,3 +1,3 @@
-0,1,2,3,4,5
-6,7,8,9,10,11
-12,13,14,15,16,17
+-0.000038,-1812.140143,6365.976106,-10773.875378,9441.287977,-1439.744061,-6853.112823,7433.361925,72.566660,-7518.521784,6807.858677,1586.021605,-9599.405711,10876.256865,-6406.017620,1819.600542
+-60.003590,-2811.660382,13178.399228,-30339.925631,43145.286816,-37009.839276,9438.328006,23631.637448,-38371.559954,23715.306332,9316.038362,-36903.451624,43082.749525,-30315.481998,13172.915245,-2811.186857
+100.000000,-0.013254,-0.012083,-0.281296,0.324078,-0.438216,0.063212,0.096122,0.034719,0.060176,0.153558,-0.402818,0.689494,-0.192379,0.050574,-0.011868

src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv

@@ -0,0 +1,3 @@
+-0.000038,-1812.140143,6365.976106,-10773.875378,9441.287977,-1439.744061,-6853.112823,7433.361925,72.566660,-7518.521784,6807.858677,1586.021605,-9599.405711,10876.256865,-6406.017620,1819.600542
+-60.003590,-2811.660382,13178.399228,-30339.925631,43145.286816,-37009.839276,9438.328006,23631.637448,-38371.559954,23715.306332,9316.038362,-36903.451624,43082.749525,-30315.481998,13172.915245,-2811.186857
+120.000000,-0.015905,-0.014500,-0.337555,0.388894,-0.525859,0.075855,0.115346,0.041663,0.072211,0.184269,-0.483382,0.827393,-0.230854,0.060689,-0.014241

src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv

@@ -0,0 +1,3 @@
+-0.000064,-3020.233571,10609.960177,-17956.458964,15735.479961,-2399.573434,-11421.854705,12388.936542,120.944433,-12530.869640,11346.431128,2643.369342,-15999.009519,18127.094775,-10676.696033,3032.667571
+-100.005984,-4686.100637,21963.998713,-50566.542718,71908.811359,-61683.065460,15730.546677,39386.062413,-63952.599923,39525.510553,15526.730604,-61505.752706,71804.582542,-50525.803330,21954.858741,-4685.311429
+120.000000,-0.015905,-0.014500,-0.337555,0.388894,-0.525859,0.075855,0.115346,0.041663,0.072211,0.184269,-0.483382,0.827393,-0.230854,0.060689,-0.014241