diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 1fa925f3e6..6cbc73560a 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -387,6 +387,12 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) case 5: strcpy(filename,"trajectory5.csv"); break; + case 6: + strcpy(filename,"trajectory6.csv"); + break; + case 7: + strcpy(filename,"trajectory7.csv"); + break; default: strcpy(filename,"trajectory0.csv"); @@ -403,8 +409,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/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 055abc014f..c92970b6ed 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 @@ -179,7 +179,7 @@ PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.03f); +PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f); /** * stall speed of the aircraft @@ -243,7 +243,7 @@ PARAM_DEFINE_FLOAT(DS_LIN_K_Z, 1.0f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_LIN_C_X, 0.75f); +PARAM_DEFINE_FLOAT(DS_LIN_C_X, 1.0f); /** * normalized damping coefficient of position PD-controller (body y-direction) @@ -255,7 +255,7 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_X, 0.75f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 0.75f); +PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 1.0f); /** * normalized damping coefficient of position PD-controller (body z-direction) @@ -267,7 +267,7 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 0.75f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 0.75f); +PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 1.0f); /** * acceleration feedback gain of position PD-controller (body x-direction) @@ -351,7 +351,7 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_YAW, 0.5f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 0.75f); +PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 1.0f); /** * normalized damping coefficient of attitude PD-controller (body pitch-direction) @@ -363,7 +363,7 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 0.75f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 0.75f); +PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 1.0f); /** * normalized damping coefficient of rudder (turn coordination) @@ -375,7 +375,7 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 0.75f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ROT_C_YAW, 0.75f); +PARAM_DEFINE_FLOAT(DS_ROT_C_YAW, 1.0f); // ============================= @@ -503,7 +503,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f); * * @unit * @min 0 - * @max 5 + * @max 7 * @decimal 1 * @increment 1 * @group FW DYN SOAR Control diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv index fa523e3cda..266a5bceb0 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.000,-781.421,2772.262,-4735.693,4209.317,-722.559,-2959.369,3288.484,-35.365,-3247.738,2983.669,648.617,-4130.364,4685.063,-2752.697,777.818 --24.999,-1273.790,6034.068,-14026.338,20140.020,-17540.834,4937.462,10289.891,-17012.188,10076.654,5264.646,-17851.061,20348.947,-14126.020,6064.982,-1278.551 -100.000,390.704,-1386.141,2367.764,-2104.665,361.193,1479.710,-1644.136,17.612,1623.877,-1491.804,-324.685,2065.244,-2342.660,1376.409,-388.912 +-0.000,-1562.842,5544.525,-9471.385,8418.635,-1445.117,-5918.738,6576.968,-70.730,-6495.476,5967.338,1297.233,-8260.729,9370.125,-5505.393,1555.636 +-49.998,-2547.580,12068.136,-28052.677,40280.039,-35081.668,9874.924,20579.781,-34024.375,20153.309,10529.293,-35702.122,40697.894,-28252.039,12129.965,-2557.102 +100.000,468.846,-1663.367,2841.334,-2525.597,433.449,1775.647,-1972.984,21.149,1948.651,-1790.171,-389.547,2478.280,-2811.167,1651.679,-466.694 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv new file mode 100644 index 0000000000..ac85ee87af --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv @@ -0,0 +1,3 @@ +-0.000,-1250.274,4435.620,-7577.108,6734.908,-1156.094,-4734.990,5261.574,-56.584,-5196.381,4773.871,1037.787,-6608.583,7496.100,-4404.315,1244.509 +-39.998,-2038.064,9654.509,-22442.141,32224.031,-28065.334,7899.939,16463.825,-27219.500,16122.647,8423.434,-28561.698,32558.315,-22601.632,9703.972,-2045.682 +100.000,375.075,-1330.695,2273.050,-2020.479,346.741,1420.523,-1578.366,16.905,1558.923,-1432.131,-311.713,1982.637,-2248.959,1321.355,-373.356 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv new file mode 100644 index 0000000000..7bed3894d0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv @@ -0,0 +1,3 @@ +-0.000,-937.705,3326.715,-5682.831,5051.181,-867.070,-3551.243,3946.181,-42.438,-3897.285,3580.403,778.340,-4956.437,5622.075,-3303.236,933.382 +-29.999,-1528.548,7240.882,-16831.606,24168.024,-21049.001,5924.954,12347.869,-20414.625,12091.985,6317.576,-21421.273,24418.737,-16951.224,7277.979,-1534.261 +100.000,281.305,-998.024,1704.767,-1515.360,260.034,1065.399,-1183.748,12.661,1169.194,-1074.090,-233.879,1486.993,-1686.751,991.032,-280.017