fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-10 10:48:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

added shear height param and new trajectories

Browse Source
This commit is contained in:
Marvin Harms
2022-08-20 11:43:03 +02:00
parent 8c65424b34
commit 48beb3ead0
11 changed files with 50 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+4 -2
View File
@@ -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");
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+2
View File
@@ -208,6 +208,8 @@ private:
(ParamFloat<px4::params::DS_ORIGIN_ALT>) _param_origin_alt,
// loiter params
(ParamInt<px4::params::DS_LOITER>) _param_loiter,
(ParamInt<px4::params::DS_W_HEADING>) _param_shear_heading,
(ParamFloat<px4::params::DS_W_HEIGHT>) _param_shear_height,
// thrust params
(ParamFloat<px4::params::DS_THRUST>) _param_thrust,
// RC feedthrough params
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+32
View File
@@ -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 =================
// ==============================================
src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv
+1 -1
View File
@@ -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
1 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
2 -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
3 100.000000 0.000000 -0.013254 0.000000 -0.012083 0.000000 -0.281296 0.000000 0.324078 0.000000 -0.438216 0.000000 0.063212 0.000000 0.096122 0.000000 0.034719 0.000000 0.060176 0.000000 0.153558 0.000000 -0.402818 0.000000 0.689494 0.000000 -0.192379 0.000000 0.050574 0.000000 -0.011868 0.000000
src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv
+1 -1
View File
@@ -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
1 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
2 -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
3 100.000000 0.000000 -0.013254 0.000000 -0.012083 0.000000 -0.281296 0.000000 0.324078 0.000000 -0.438216 0.000000 0.063212 0.000000 0.096122 0.000000 0.034719 0.000000 0.060176 0.000000 0.153558 0.000000 -0.402818 0.000000 0.689494 0.000000 -0.192379 0.000000 0.050574 0.000000 -0.011868 0.000000
src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv
+1 -1
View File
@@ -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
1 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
2 -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
3 120.000000 20.000000 -0.015905 -0.002651 -0.014500 -0.002417 -0.337555 -0.056259 0.388894 0.064816 -0.525859 -0.087643 0.075855 0.012642 0.115346 0.019224 0.041663 0.006944 0.072211 0.012035 0.184269 0.030712 -0.483382 -0.080564 0.827393 0.137899 -0.230854 -0.038476 0.060689 0.010115 -0.014241 -0.002374
src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv
+1 -1
View File
@@ -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
1 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
2 -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
3 120.000000 20.000000 -0.015905 -0.002651 -0.014500 -0.002417 -0.337555 -0.056259 0.388894 0.064816 -0.525859 -0.087643 0.075855 0.012642 0.115346 0.019224 0.041663 0.006944 0.072211 0.012035 0.184269 0.030712 -0.483382 -0.080564 0.827393 0.137899 -0.230854 -0.038476 0.060689 0.010115 -0.014241 -0.002374
src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv
+1 -1
View File
@@ -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
1 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
2 -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
3 99.999981 -0.000019 -906.083325 -906.070071 3182.975970 3182.988053 -5387.218985 -5386.937689 4720.968066 4720.643988 -720.310246 -719.872030 -3426.493199 -3426.556411 3716.777085 3716.680963 36.318049 36.283330 -3759.200716 -3759.260892 3404.082896 3403.929338 792.607985 793.010803 -4799.013362 -4799.702856 5437.936054 5438.128432 -3202.958236 -3203.008810 909.788403 909.800271
src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv
+1 -1
View File
@@ -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
1 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
2 -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
3 99.999987 -0.000008 -604.059968 -362.428029 2121.979952 1273.195221 -3591.573089 -2154.775076 3147.420070 1888.257595 -480.352903 -287.948812 -2284.307729 -1370.622565 2477.883430 1486.672385 24.223606 14.513332 -2506.113752 -1503.704357 2269.439783 1361.571735 528.271050 317.204321 -3199.112410 -1919.881142 3625.226576 2175.251373 -2135.288633 -1281.203524 606.521646 363.920108
src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv
+3 -3
View File
@@ -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
1 0.000025 0.000016 1208.093428 755.058393 -4243.984071 -2652.490044 7182.583586 4489.114741 -6294.191984 -3933.869990 959.829374 599.893359 4568.741882 2855.463676 -4955.574617 -3097.234136 -48.377773 -30.236108 5012.347856 3132.717410 -4538.572451 -2836.607782 -1057.347737 -660.842335 6399.603807 3999.752380 -7250.837910 -4531.773694 4270.678413 2669.174008 -1213.067028 -758.166893
2 -39.999901 -24.999938 -1874.440255 -1171.525159 8785.599485 5490.999678 -20226.617094 -12641.635684 28763.524552 17977.202845 -24673.226195 -15420.766372 6292.218672 3932.636670 15754.424968 9846.515605 -25581.039968 -15988.149980 15810.204223 9881.377639 6210.692245 3881.682653 -24602.301093 -15376.438183 28721.833034 17951.145646 -20210.321337 -12631.450836 8781.943498 5488.714686 -1874.124572 -1171.327857
3 99.999992 -0.000005 -362.441283 -226.517518 1273.183138 795.747013 -2155.056372 -1346.734422 1888.581673 1180.160997 -288.387028 -179.968008 -1370.559352 -856.639103 1486.768507 929.170241 14.548051 9.070832 -1503.644181 -939.815223 1361.725293 850.982335 316.801503 198.252701 -1919.191648 -1199.925714 2175.058994 1359.532108 -1281.152950 -800.752202 363.908241 227.450068
src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv
+3 -3
View File
@@ -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
1 33.279737 25.647286 2355.965009 1322.946130 -9853.856406 -6288.219118 19962.238258 15695.694585 -24975.068263 -26100.811348 19045.215851 29190.042729 -4528.565470 -17320.882010 -10036.742015 -7206.280655 16387.989801 28417.502641 -11464.538539 -27570.615375 -2278.904099 1092.280900 17753.913430 32845.587042 -26381.541512 -49815.421890 23421.451272 41317.083747 -12798.876432 -20391.608674 3352.799155 4792.959996
2 -1.655556 -4.022804 6955.844338 5578.254991 -37836.458976 -29867.374126 101707.858193 78424.519794 -169407.588331 -126648.076540 177059.774008 126732.911714 -83213.665962 -53339.402193 -70079.333339 -56007.887034 169854.280775 119673.494918 -127407.321325 -82844.941058 -32264.742913 -28429.087152 186788.856870 129249.441369 -229810.067680 -153381.353797 162394.940106 106507.489754 -67738.519051 -44042.229641 13090.706363 8473.887609
3 90.256827 -7.320205 695.796077 2064.611816 -3566.592598 -11998.518948 8070.766654 33890.176305 -9651.849655 -59497.114479 4159.119473 66640.989601 5301.738775 -36741.103291 -9269.047782 -20637.068567 2572.483886 64946.871726 7898.748320 -56132.549319 -9780.034074 -5332.110902 -66.886050 73702.059578 11734.271409 -99372.156096 -14380.432089 74574.323597 8489.722642 -32725.856186 -2195.005657 6681.314060