From 48beb3ead0b398e2a880d856d3699d5a1674ca14 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Sat, 20 Aug 2022 11:43:03 +0200 Subject: [PATCH] added shear height param and new trajectories --- .../FixedwingPositionINDIControl.cpp | 6 ++-- .../FixedwingPositionINDIControl.hpp | 2 ++ .../fw_dyn_soar_control_params.c | 32 +++++++++++++++++++ .../trajectories/trajectory0.csv | 2 +- .../trajectories/trajectory1.csv | 2 +- .../trajectories/trajectory2.csv | 2 +- .../trajectories/trajectory3.csv | 2 +- .../trajectories/trajectory4.csv | 2 +- .../trajectories/trajectory5.csv | 2 +- .../trajectories/trajectory6.csv | 6 ++-- .../trajectories/trajectory7.csv | 6 ++-- 11 files changed, 50 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 62b5dcf075..4d9ae4b195 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -173,6 +173,8 @@ FixedwingPositionINDIControl::parameters_update() _loiter = _param_loiter.get(); + _shear_heading = _param_shear_heading.get()/180.f * M_PI_F + M_PI_2_F; + _shear_h_ref = _param_shear_height.get(); _select_trajectory(0.0f); _thrust = _param_thrust.get(); @@ -535,8 +537,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) // ======================================================================= bool error = false; - char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"; - //char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/"; + //char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"; + char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/"; //PX4_ERR(home_dir); strcat(home_dir,filename); FILE* fp = fopen(home_dir, "r"); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 57a14e7608..8bc11a7f3c 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -208,6 +208,8 @@ private: (ParamFloat) _param_origin_alt, // loiter params (ParamInt) _param_loiter, + (ParamInt) _param_shear_heading, + (ParamFloat) _param_shear_height, // thrust params (ParamFloat) _param_thrust, // RC feedthrough params diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 4869fc9435..3b09228281 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -502,6 +502,38 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f); */ PARAM_DEFINE_INT32(DS_LOITER, 0); +// ====================================================== +// ============ wind shear heading ====================== +// ====================================================== + +/** + * integer defining wind heading + * + * @unit deg + * @min -180 + * @max 180 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_W_HEADING, 0); + +// ====================================================== +// ======= wind shear height in soaring frame =========== +// ====================================================== + +/** + * float defining wind shear vertical postition in soaring frame + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f); + // ============================================== // ============== engine thrust ================= // ============================================== diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv index 240d82f423..f43db57906 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 -99.999753,-4686.100638,21963.998712,-50566.542735,71908.811380,-61683.065487,15730.546681,39386.062419,-63952.599921,39525.510557,15526.730614,-61505.752731,71804.582585,-50525.803342,21954.858744,-4685.311430 -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 +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,0.000000 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv index 52f729827d..0083d14f4a 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.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 -59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858 -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 +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,0.000000 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv index acc5304325..c33415e34c 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv @@ -1,3 +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 -59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858 -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 +20.000000,-0.002651,-0.002417,-0.056259,0.064816,-0.087643,0.012642,0.019224,0.006944,0.012035,0.030712,-0.080564,0.137899,-0.038476,0.010115,-0.002374 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv index f9838f54b4..82cde96a85 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.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 -99.999753,-4686.100638,21963.998712,-50566.542735,71908.811380,-61683.065487,15730.546681,39386.062419,-63952.599921,39525.510557,15526.730614,-61505.752731,71804.582585,-50525.803342,21954.858744,-4685.311430 -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 +20.000000,-0.002651,-0.002417,-0.056259,0.064816,-0.087643,0.012642,0.019224,0.006944,0.012035,0.030712,-0.080564,0.137899,-0.038476,0.010115,-0.002374 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv index 0347087ed5..71a8ec8126 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv @@ -1,3 +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 -59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858 -99.999981,-906.083325,3182.975970,-5387.218985,4720.968066,-720.310246,-3426.493199,3716.777085,36.318049,-3759.200716,3404.082896,792.607985,-4799.013362,5437.936054,-3202.958236,909.788403 +-0.000019,-906.070071,3182.988053,-5386.937689,4720.643988,-719.872030,-3426.556411,3716.680963,36.283330,-3759.260892,3403.929338,793.010803,-4799.702856,5438.128432,-3203.008810,909.800271 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv index 7b8334eab1..964e032f5d 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv @@ -1,3 +1,3 @@ 0.000025,1208.093428,-4243.984071,7182.583586,-6294.191984,959.829374,4568.741882,-4955.574617,-48.377773,5012.347856,-4538.572451,-1057.347737,6399.603807,-7250.837910,4270.678413,-1213.067028 -39.999901,-1874.440255,8785.599485,-20226.617094,28763.524552,-24673.226195,6292.218672,15754.424968,-25581.039968,15810.204223,6210.692245,-24602.301093,28721.833034,-20210.321337,8781.943498,-1874.124572 -99.999987,-604.059968,2121.979952,-3591.573089,3147.420070,-480.352903,-2284.307729,2477.883430,24.223606,-2506.113752,2269.439783,528.271050,-3199.112410,3625.226576,-2135.288633,606.521646 +-0.000008,-362.428029,1273.195221,-2154.775076,1888.257595,-287.948812,-1370.622565,1486.672385,14.513332,-1503.704357,1361.571735,317.204321,-1919.881142,2175.251373,-1281.203524,363.920108 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv index bc32e9dd02..da5d2bb0b4 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv @@ -1,3 +1,3 @@ -0.000025,1208.093428,-4243.984071,7182.583586,-6294.191984,959.829374,4568.741882,-4955.574617,-48.377773,5012.347856,-4538.572451,-1057.347737,6399.603807,-7250.837910,4270.678413,-1213.067028 --39.999901,-1874.440255,8785.599485,-20226.617094,28763.524552,-24673.226195,6292.218672,15754.424968,-25581.039968,15810.204223,6210.692245,-24602.301093,28721.833034,-20210.321337,8781.943498,-1874.124572 -99.999992,-362.441283,1273.183138,-2155.056372,1888.581673,-288.387028,-1370.559352,1486.768507,14.548051,-1503.644181,1361.725293,316.801503,-1919.191648,2175.058994,-1281.152950,363.908241 +0.000016,755.058393,-2652.490044,4489.114741,-3933.869990,599.893359,2855.463676,-3097.234136,-30.236108,3132.717410,-2836.607782,-660.842335,3999.752380,-4531.773694,2669.174008,-758.166893 +-24.999938,-1171.525159,5490.999678,-12641.635684,17977.202845,-15420.766372,3932.636670,9846.515605,-15988.149980,9881.377639,3881.682653,-15376.438183,17951.145646,-12631.450836,5488.714686,-1171.327857 +-0.000005,-226.517518,795.747013,-1346.734422,1180.160997,-179.968008,-856.639103,929.170241,9.070832,-939.815223,850.982335,198.252701,-1199.925714,1359.532108,-800.752202,227.450068 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv index 033fdc6449..4135794c29 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv @@ -1,3 +1,3 @@ -33.279737,2355.965009,-9853.856406,19962.238258,-24975.068263,19045.215851,-4528.565470,-10036.742015,16387.989801,-11464.538539,-2278.904099,17753.913430,-26381.541512,23421.451272,-12798.876432,3352.799155 --1.655556,6955.844338,-37836.458976,101707.858193,-169407.588331,177059.774008,-83213.665962,-70079.333339,169854.280775,-127407.321325,-32264.742913,186788.856870,-229810.067680,162394.940106,-67738.519051,13090.706363 -90.256827,695.796077,-3566.592598,8070.766654,-9651.849655,4159.119473,5301.738775,-9269.047782,2572.483886,7898.748320,-9780.034074,-66.886050,11734.271409,-14380.432089,8489.722642,-2195.005657 +25.647286,1322.946130,-6288.219118,15695.694585,-26100.811348,29190.042729,-17320.882010,-7206.280655,28417.502641,-27570.615375,1092.280900,32845.587042,-49815.421890,41317.083747,-20391.608674,4792.959996 +-4.022804,5578.254991,-29867.374126,78424.519794,-126648.076540,126732.911714,-53339.402193,-56007.887034,119673.494918,-82844.941058,-28429.087152,129249.441369,-153381.353797,106507.489754,-44042.229641,8473.887609 +-7.320205,2064.611816,-11998.518948,33890.176305,-59497.114479,66640.989601,-36741.103291,-20637.068567,64946.871726,-56132.549319,-5332.110902,73702.059578,-99372.156096,74574.323597,-32725.856186,6681.314060