From c44c387b6d6f3ec4ee228eb7ad048d27032c83eb Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Mon, 6 Jun 2022 11:47:23 +0200 Subject: [PATCH] changed basis fun sigma back to 1.0 --- .../fw_dyn_soar_control/FixedwingPositionINDIControl.cpp | 7 +++++-- .../fw_dyn_soar_control/trajectories/trajectory0.csv | 2 +- .../trajectories/trajectory0_sigma_1.csv | 3 +++ .../fw_dyn_soar_control/trajectories/trajectory1.csv | 6 +++--- .../fw_dyn_soar_control/trajectories/trajectory2.csv | 3 +++ .../fw_dyn_soar_control/trajectories/trajectory3.csv | 3 +++ 6 files changed, 18 insertions(+), 6 deletions(-) create mode 100644 src/modules/fw_dyn_soar_control/trajectories/trajectory0_sigma_1.csv create mode 100644 src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv create mode 100644 src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 85d030ae1d..1462e88cbb 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/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 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"); diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv index 7df166f0e7..ce8c8c3937 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv +++ b/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 \ No newline at end of file +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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0_sigma_1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0_sigma_1.csv new file mode 100644 index 0000000000..7df166f0e7 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv index 662e2e6848..80248ac32a 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv +++ b/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 \ No newline at end of file +-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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv new file mode 100644 index 0000000000..16ed1b1198 --- /dev/null +++ b/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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv new file mode 100644 index 0000000000..4f8a1fd753 --- /dev/null +++ b/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