diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 415c39a62d..8cab1f995c 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -119,5 +119,5 @@ if(REPLAY_FILE) message(STATUS "Building without lockstep for replay") set(ENABLE_LOCKSTEP_SCHEDULER no) else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) + set(ENABLE_LOCKSTEP_SCHEDULER no) endif() diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 7edde4ca4e..aaff3cc388 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -57,8 +57,9 @@ FixedwingPositionINDIControl::FixedwingPositionINDIControl() : _attitude_sp_pub(ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { - // limit to 50 Hz - //_vehicle_angular_velocity_sub.set_interval_ms(20); + // limit to 100 Hz + _vehicle_angular_velocity_sub.set_interval_ms(1000/_sample_frequency); + /* fetch initial parameter values */ parameters_update(); @@ -381,68 +382,12 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) void FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) { - /* - // File pointer - std::ifstream fin; - - // Open an existing file for testing - //fin.open("/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"+filename, ios::in); - // Open an existing file from SD card - fin.open(PX4_ROOTFSDIR"/fs/microsd/trajectories/"+filename); - - // Read the Data from the file - // as String Vector - string line, word; - - bool error = false; - - // loop over x, y, z - for (int l=0; l<3; l++) { - getline(fin, line, '\n'); - - // used for breaking words - stringstream s(line); - - // read every column data of a row and - // store it in a string variable, 'word' - uint i = 0; - while (getline(s, word, ',')) { - // catch error - if(i>=_num_basis_funs){ - PX4_ERR("number of coefficients is too large."); - error = true; - } - else{ - switch(l){ - case 0: - _basis_coeffs_x(i) = stof(word); - break; - case 1: - _basis_coeffs_y(i) = stof(word); - break; - case 2: - _basis_coeffs_z(i) = stof(word); - break; - } - - } - //PX4_INFO("coefficient value: %.6f", (double)stof(word)); - i += 1; - } - // catch error - if(_num_basis_funs-i>1){ - PX4_ERR("number of coefficients is too small: %.f", (double)(i)); - error = true; - } - } - fin.close(); - */ // ======================================================================= 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"); @@ -485,7 +430,7 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) default: break; } - PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); + //PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); value = strtok(NULL, ","); column++; @@ -695,7 +640,6 @@ FixedwingPositionINDIControl::Run() _angular_accel_sp.xyz[1] = ctrl(1); _angular_accel_sp.xyz[2] = ctrl(2); _angular_accel_sp_pub.publish(_angular_accel_sp); - //print_message(_angular_accel_sp); // ========================= // publish attitude setpoint @@ -1131,7 +1075,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve // ========================================== // input meant for tuning the INDI controller // ========================================== - /* + if(hrt_absolute_time()%2000000>1000000){ rot_acc_command = Vector3f{2.0f,1.f,0.f}; //rot_acc_command = Vector3f{0.f,0.f,0.5f}; @@ -1140,7 +1084,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve rot_acc_command = Vector3f{-2.0f,-1.f,0.f}; //rot_acc_command = Vector3f{0.f,0.f,-0.5f}; } - */ + //PX4_INFO("force command: \t%.2f", (double)(f_command*f_command)); //PX4_INFO("force command: \t%.2f\t%.2f\t%.2f", (double)f_command(0), (double)f_command(1), (double)f_command(2)); //PX4_INFO("FRD body frame rotation vec: \t%.2f\t%.2f\t%.2f", (double)w_err(0), (double)w_err(1), (double)w_err(2)); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index b4874da1fb..ccc956a7d8 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -156,56 +156,56 @@ private: // parameter struct DEFINE_PARAMETERS( // aircraft params - (ParamFloat) _param_fw_inertia_roll, - (ParamFloat) _param_fw_inertia_pitch, - (ParamFloat) _param_fw_inertia_yaw, - (ParamFloat) _param_fw_mass, - (ParamFloat) _param_fw_wing_area, - (ParamFloat) _param_rho, + (ParamFloat) _param_fw_inertia_roll, + (ParamFloat) _param_fw_inertia_pitch, + (ParamFloat) _param_fw_inertia_yaw, + (ParamFloat) _param_fw_mass, + (ParamFloat) _param_fw_wing_area, + (ParamFloat) _param_rho, // aerodynamic params - (ParamFloat) _param_fw_c_l0, - (ParamFloat) _param_fw_c_l1, - (ParamFloat) _param_fw_c_d0, - (ParamFloat) _param_fw_c_d1, - (ParamFloat) _param_fw_c_d2, - (ParamFloat) _param_aoa_offset, + (ParamFloat) _param_fw_c_l0, + (ParamFloat) _param_fw_c_l1, + (ParamFloat) _param_fw_c_d0, + (ParamFloat) _param_fw_c_d1, + (ParamFloat) _param_fw_c_d2, + (ParamFloat) _param_aoa_offset, // filter params - (ParamFloat) _param_filter_a1, - (ParamFloat) _param_filter_a2, - (ParamFloat) _param_filter_b1, - (ParamFloat) _param_filter_b2, - (ParamFloat) _param_filter_b3, + (ParamFloat) _param_filter_a1, + (ParamFloat) _param_filter_a2, + (ParamFloat) _param_filter_b1, + (ParamFloat) _param_filter_b2, + (ParamFloat) _param_filter_b3, // controller params - (ParamFloat) _param_k_x_roll, - (ParamFloat) _param_k_x_pitch, - (ParamFloat) _param_k_x_yaw, - (ParamFloat) _param_k_v_roll, - (ParamFloat) _param_k_v_pitch, - (ParamFloat) _param_k_v_yaw, - (ParamFloat) _param_k_a_roll, - (ParamFloat) _param_k_a_pitch, - (ParamFloat) _param_k_a_yaw, - (ParamFloat) _param_k_q_roll, - (ParamFloat) _param_k_q_pitch, - (ParamFloat) _param_k_q_yaw, - (ParamFloat) _param_k_w_roll, - (ParamFloat) _param_k_w_pitch, - (ParamFloat) _param_k_w_yaw, + (ParamFloat) _param_k_x_roll, + (ParamFloat) _param_k_x_pitch, + (ParamFloat) _param_k_x_yaw, + (ParamFloat) _param_k_v_roll, + (ParamFloat) _param_k_v_pitch, + (ParamFloat) _param_k_v_yaw, + (ParamFloat) _param_k_a_roll, + (ParamFloat) _param_k_a_pitch, + (ParamFloat) _param_k_a_yaw, + (ParamFloat) _param_k_q_roll, + (ParamFloat) _param_k_q_pitch, + (ParamFloat) _param_k_q_yaw, + (ParamFloat) _param_k_w_roll, + (ParamFloat) _param_k_w_pitch, + (ParamFloat) _param_k_w_yaw, // low-level controller params - (ParamFloat) _param_k_act_roll, - (ParamFloat) _param_k_act_pitch, - (ParamFloat) _param_k_act_yaw, - (ParamFloat) _param_k_damping_roll, - (ParamFloat) _param_k_damping_pitch, - (ParamFloat) _param_k_damping_yaw, + (ParamFloat) _param_k_act_roll, + (ParamFloat) _param_k_act_pitch, + (ParamFloat) _param_k_act_yaw, + (ParamFloat) _param_k_damping_roll, + (ParamFloat) _param_k_damping_pitch, + (ParamFloat) _param_k_damping_yaw, // location params - (ParamFloat) _param_origin_lat, - (ParamFloat) _param_origin_lon, - (ParamFloat) _param_origin_alt, + (ParamFloat) _param_origin_lat, + (ParamFloat) _param_origin_lon, + (ParamFloat) _param_origin_alt, // loiter params - (ParamInt) _param_loiter, + (ParamInt) _param_loiter, // thrust params - (ParamFloat) _param_thrust + (ParamFloat) _param_thrust ) @@ -298,7 +298,7 @@ private: Vector _a_lpf_list; Vector _l_lpf_list; // controller frequency - const float _sample_frequency = 250.f; + const float _sample_frequency = 100.f; // Low-Pass filters stage 1 const float _cutoff_frequency_1 = 5.f; math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // linear acceleration 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 50d0223e0e..60ddea4574 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 @@ -22,7 +22,7 @@ * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FW_INERTIA_ROLL, 0.197563f); +PARAM_DEFINE_FLOAT(DS_INERTIA_ROLL, 0.197563f); /** * inertia around body y-axis @@ -36,7 +36,7 @@ PARAM_DEFINE_FLOAT(FW_INERTIA_ROLL, 0.197563f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FW_INERTIA_PITCH, 0.1458929f); +PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f); /** * inertia around body z-axis @@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(FW_INERTIA_PITCH, 0.1458929f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FW_INERTIA_YAW, 0.1477f); +PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f); @@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(FW_INERTIA_YAW, 0.1477f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FW_MASS, 1.4f); +PARAM_DEFINE_FLOAT(DS_MASS, 1.4f); /** * total wing area used for lift and drag computation @@ -78,7 +78,7 @@ PARAM_DEFINE_FLOAT(FW_MASS, 1.4f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FW_WING_AREA, 0.4f); +PARAM_DEFINE_FLOAT(DS_WING_AREA, 0.4f); /** @@ -91,7 +91,7 @@ PARAM_DEFINE_FLOAT(FW_WING_AREA, 0.4f); * @increment 0.001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(RHO, 1.223f); +PARAM_DEFINE_FLOAT(DS_RHO, 1.223f); /** * estimated lift coefficients used for lift and drag computation @@ -106,7 +106,7 @@ PARAM_DEFINE_FLOAT(RHO, 1.223f); * @increment 0.001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(C_L0, 0.356f); +PARAM_DEFINE_FLOAT(DS_C_L0, 0.356f); /** * estimated lift coefficients used for lift and drag computation @@ -121,7 +121,7 @@ PARAM_DEFINE_FLOAT(C_L0, 0.356f); * @increment 0.001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(C_L1, 2.354f); +PARAM_DEFINE_FLOAT(DS_C_L1, 2.354f); /** @@ -137,7 +137,7 @@ PARAM_DEFINE_FLOAT(C_L1, 2.354f); * @increment 0.0001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(C_D0, 0.0288f); +PARAM_DEFINE_FLOAT(DS_C_D0, 0.0288f); /** * estimated drag coefficients used for lift and drag computation @@ -152,7 +152,7 @@ PARAM_DEFINE_FLOAT(C_D0, 0.0288f); * @increment 0.0001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(C_D1, 0.3783f); +PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f); /** * estimated drag coefficients used for lift and drag computation @@ -167,7 +167,7 @@ PARAM_DEFINE_FLOAT(C_D1, 0.3783f); * @increment 0.0001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(C_D2, 1.984f); +PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f); /** * offset angle between body frame (Pixhawk) and the wing chord @@ -181,7 +181,7 @@ PARAM_DEFINE_FLOAT(C_D2, 1.984f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(AOA_OFFSET, 0.01f); +PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.01f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -193,7 +193,7 @@ PARAM_DEFINE_FLOAT(AOA_OFFSET, 0.01f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_A1, 1.16826067f); +PARAM_DEFINE_FLOAT(DS_FILTER_A1, 1.16826067f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -205,7 +205,7 @@ PARAM_DEFINE_FLOAT(FILTER_A1, 1.16826067f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_A2, -0.42411821f); +PARAM_DEFINE_FLOAT(DS_FILTER_A2, -0.42411821f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -217,7 +217,7 @@ PARAM_DEFINE_FLOAT(FILTER_A2, -0.42411821f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_B1, 0.06396438f); +PARAM_DEFINE_FLOAT(DS_FILTER_B1, 0.06396438f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -229,7 +229,7 @@ PARAM_DEFINE_FLOAT(FILTER_B1, 0.06396438f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_B2, 0.12792877f); +PARAM_DEFINE_FLOAT(DS_FILTER_B2, 0.12792877f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -241,7 +241,7 @@ PARAM_DEFINE_FLOAT(FILTER_B2, 0.12792877f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_B3, 0.06396438f); +PARAM_DEFINE_FLOAT(DS_FILTER_B3, 0.06396438f); // ======================================================== // =================== CONTROL GAINS ====================== @@ -256,7 +256,7 @@ PARAM_DEFINE_FLOAT(FILTER_B3, 0.06396438f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_X_ROLL, 1.0f); +PARAM_DEFINE_FLOAT(DS_K_X_ROLL, 1.0f); /** * pitch gain of K_x (position error gain) @@ -268,7 +268,7 @@ PARAM_DEFINE_FLOAT(K_X_ROLL, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_X_PITCH, 1.0f); +PARAM_DEFINE_FLOAT(DS_K_X_PITCH, 1.0f); /** * yaw gain of K_x (position error gain) @@ -280,7 +280,7 @@ PARAM_DEFINE_FLOAT(K_X_PITCH, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_X_YAW, 1.0f); +PARAM_DEFINE_FLOAT(DS_K_X_YAW, 1.0f); /** * roll gain of K_v (velocity error gain) @@ -292,7 +292,7 @@ PARAM_DEFINE_FLOAT(K_X_YAW, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_V_ROLL, 1.3f); +PARAM_DEFINE_FLOAT(DS_K_V_ROLL, 1.3f); /** * pitch gain of K_v (velocity error gain) @@ -304,7 +304,7 @@ PARAM_DEFINE_FLOAT(K_V_ROLL, 1.3f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_V_PITCH, 1.3f); +PARAM_DEFINE_FLOAT(DS_K_V_PITCH, 1.3f); /** * yaw gain of K_v (velocity error gain) @@ -316,7 +316,7 @@ PARAM_DEFINE_FLOAT(K_V_PITCH, 1.3f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_V_YAW, 1.3f); +PARAM_DEFINE_FLOAT(DS_K_V_YAW, 1.3f); /** * roll gain of K_A (acceleration error gain) @@ -328,7 +328,7 @@ PARAM_DEFINE_FLOAT(K_V_YAW, 1.3f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_A_ROLL, 0.5f); +PARAM_DEFINE_FLOAT(DS_K_A_ROLL, 0.5f); /** * pitch gain of K_A (acceleration error gain) @@ -340,7 +340,7 @@ PARAM_DEFINE_FLOAT(K_A_ROLL, 0.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_A_PITCH, 0.5f); +PARAM_DEFINE_FLOAT(DS_K_A_PITCH, 0.5f); /** * yaw gain of K_A (acceleration error gain) @@ -352,7 +352,7 @@ PARAM_DEFINE_FLOAT(K_A_PITCH, 0.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_A_YAW, 0.5f); +PARAM_DEFINE_FLOAT(DS_K_A_YAW, 0.5f); /** * roll gain of K_Q (attitude error gain) @@ -364,7 +364,7 @@ PARAM_DEFINE_FLOAT(K_A_YAW, 0.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_Q_ROLL, 10.0f); +PARAM_DEFINE_FLOAT(DS_K_Q_ROLL, 10.0f); /** * pitch gain of K_Q (attitude error gain) @@ -376,7 +376,7 @@ PARAM_DEFINE_FLOAT(K_Q_ROLL, 10.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_Q_PITCH, 10.0f); +PARAM_DEFINE_FLOAT(DS_K_Q_PITCH, 10.0f); /** * yaw gain of K_Q (attitude error gain) @@ -388,7 +388,7 @@ PARAM_DEFINE_FLOAT(K_Q_PITCH, 10.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_Q_YAW, 2.0f); +PARAM_DEFINE_FLOAT(DS_K_Q_YAW, 2.0f); /** * roll gain of K_W (angular velocity error gain) @@ -400,7 +400,7 @@ PARAM_DEFINE_FLOAT(K_Q_YAW, 2.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_W_ROLL, 5.0f); +PARAM_DEFINE_FLOAT(DS_K_W_ROLL, 5.0f); /** * pitch gain of K_W (angular velocity error gain) @@ -412,7 +412,7 @@ PARAM_DEFINE_FLOAT(K_W_ROLL, 5.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f); +PARAM_DEFINE_FLOAT(DS_K_W_PITCH, 5.0f); /** * yaw gain of K_W (angular velocity error gain) @@ -424,7 +424,7 @@ PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f); +PARAM_DEFINE_FLOAT(DS_K_W_YAW, 1.0f); // ============================= // low level INDI control params @@ -440,7 +440,7 @@ PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f); +PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.1f); /** * pitch gain of K_ACT (actuator deflection gain) @@ -452,7 +452,7 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f); +PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.03f); /** * yaw gain of K_ACT (actuator deflection gain) @@ -464,7 +464,7 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f); +PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f); /** * roll gain of K_ACT_DAMPING (actuator damping gain) @@ -476,7 +476,7 @@ PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f); +PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f); /** * pitch gain of K_ACT_DAMPING (actuator damping gain) @@ -488,7 +488,7 @@ PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f); +PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.01f); /** * yaw gain of K_ACT_DAMPING (actuator damping gain) @@ -500,7 +500,7 @@ PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_DAMPING_YAW, 0.0f); +PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f); // =================================================== // ============== trajectory center ================= @@ -516,7 +516,7 @@ PARAM_DEFINE_FLOAT(K_DAMPING_YAW, 0.0f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(ORIGIN_LAT, 47.39797f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.39797f); /** * longitude of trajectory start point (WGS84) @@ -528,7 +528,7 @@ PARAM_DEFINE_FLOAT(ORIGIN_LAT, 47.39797f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(ORIGIN_LON, 8.54554f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.54554f); /** * altitude of trajectory start point (WGS84) @@ -540,7 +540,7 @@ PARAM_DEFINE_FLOAT(ORIGIN_LON, 8.54554f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(ORIGIN_ALT, 488.0f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 488.0f); // ====================================================== @@ -557,7 +557,7 @@ PARAM_DEFINE_FLOAT(ORIGIN_ALT, 488.0f); * @increment 1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_INT32(LOITER, 0); +PARAM_DEFINE_INT32(DS_LOITER, 0); // ============================================== // ============== engine thrust ================= @@ -572,4 +572,4 @@ PARAM_DEFINE_INT32(LOITER, 0); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(THRUST, 0); \ No newline at end of file +PARAM_DEFINE_FLOAT(DS_THRUST, 0); \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv index acfd4d22af..8af63eaa40 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.000014,-3125.684664,11089.049565,-18942.770235,16837.269583,-2890.234017,-11837.475189,13153.935749,-141.459060,-12990.951555,11934.676728,2594.466297,-16521.457469,18740.250392,-11010.786326,3111.272558 --99.995573,-5095.160841,24136.272616,-56105.353355,80560.078564,-70163.335913,19749.848200,41159.562505,-68048.750290,40306.617839,21058.585202,-71404.243825,81395.788686,-56504.078947,24259.929878,-5114.204474 -100.000000,-0.006882,-0.009535,-0.081985,-0.006197,-0.086599,0.025787,0.106096,-0.069921,0.008318,0.030489,-0.376848,0.061624,-0.128956,0.060856,-0.002952 +-0.000,-3125.685,11089.050,-18942.770,16837.270,-2890.234,-11837.475,13153.936,-141.459,-12990.952,11934.677,2594.466,-16521.457,18740.250,-11010.786,3111.273 +-99.996,-5095.161,24136.273,-56105.353,80560.079,-70163.336,19749.848,41159.563,-68048.750,40306.618,21058.585,-71404.244,81395.789,-56504.079,24259.930,-5114.204 +100.000,-0.007,-0.010,-0.082,-0.006,-0.087,0.026,0.106,-0.070,0.008,0.030,-0.377,0.062,-0.129,0.061,-0.003 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv index eee5c54968..8eab16b5fa 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.000008,-1875.410798,6653.429739,-11365.662141,10102.361750,-1734.140410,-7102.485113,7892.361450,-84.875436,-7794.570933,7160.806037,1556.679778,-9912.874481,11244.150235,-6606.471795,1866.763535 --59.997344,-3057.096504,14481.763570,-33663.212013,48336.047139,-42098.001548,11849.908920,24695.737503,-40829.250174,24183.970704,12635.151121,-42842.546295,48837.473212,-33902.447368,14555.957927,-3068.522684 -100.000000,-0.006882,-0.009535,-0.081985,-0.006197,-0.086599,0.025787,0.106096,-0.069921,0.008318,0.030489,-0.376848,0.061624,-0.128956,0.060856,-0.002952 +-0.000,-1875.411,6653.430,-11365.662,10102.362,-1734.140,-7102.485,7892.361,-84.875,-7794.571,7160.806,1556.680,-9912.874,11244.150,-6606.472,1866.764 +-59.997,-3057.097,14481.764,-33663.212,48336.047,-42098.002,11849.909,24695.738,-40829.250,24183.971,12635.151,-42842.546,48837.473,-33902.447,14555.958,-3068.523 +100.000,-0.007,-0.010,-0.082,-0.006,-0.087,0.026,0.106,-0.070,0.008,0.030,-0.377,0.062,-0.129,0.061,-0.003 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv index c84f1bb195..450343e8fd 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.000008,-1875.410798,6653.429739,-11365.662141,10102.361750,-1734.140410,-7102.485113,7892.361450,-84.875436,-7794.570933,7160.806037,1556.679778,-9912.874481,11244.150235,-6606.471795,1866.763535 --59.997344,-3057.096504,14481.763570,-33663.212013,48336.047139,-42098.001548,11849.908920,24695.737503,-40829.250174,24183.970704,12635.151121,-42842.546295,48837.473212,-33902.447368,14555.957927,-3068.522684 -120.000000,-0.008258,-0.011442,-0.098382,-0.007437,-0.103918,0.030944,0.127315,-0.083905,0.009981,0.036587,-0.452217,0.073949,-0.154747,0.073028,-0.003542 +-0.000,-1875.411,6653.430,-11365.662,10102.362,-1734.140,-7102.485,7892.361,-84.875,-7794.571,7160.806,1556.680,-9912.874,11244.150,-6606.472,1866.764 +-59.997,-3057.097,14481.764,-33663.212,48336.047,-42098.002,11849.909,24695.738,-40829.250,24183.971,12635.151,-42842.546,48837.473,-33902.447,14555.958,-3068.523 +120.000,-0.008,-0.011,-0.098,-0.007,-0.104,0.031,0.127,-0.084,0.010,0.037,-0.452,0.074,-0.155,0.073,-0.004 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv index 62316cfa89..94cb374efe 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.000014,-3125.684664,11089.049565,-18942.770235,16837.269583,-2890.234017,-11837.475189,13153.935749,-141.459060,-12990.951555,11934.676728,2594.466297,-16521.457469,18740.250392,-11010.786326,3111.272558 --99.995573,-5095.160841,24136.272616,-56105.353355,80560.078564,-70163.335913,19749.848200,41159.562505,-68048.750290,40306.617839,21058.585202,-71404.243825,81395.788686,-56504.078947,24259.929878,-5114.204474 -120.000000,-0.008258,-0.011442,-0.098382,-0.007437,-0.103918,0.030944,0.127315,-0.083905,0.009981,0.036587,-0.452217,0.073949,-0.154747,0.073028,-0.003542 +-0.000,-3125.685,11089.050,-18942.770,16837.270,-2890.234,-11837.475,13153.936,-141.459,-12990.952,11934.677,2594.466,-16521.457,18740.250,-11010.786,3111.273 +-99.996,-5095.161,24136.273,-56105.353,80560.079,-70163.336,19749.848,41159.563,-68048.750,40306.618,21058.585,-71404.244,81395.789,-56504.079,24259.930,-5114.204 +120.000,-0.008,-0.011,-0.098,-0.007,-0.104,0.031,0.127,-0.084,0.010,0.037,-0.452,0.074,-0.155,0.073,-0.004 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv index eb78f07b26..1bd59afe96 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.000007,-1562.842332,5544.524783,-9471.385118,8418.634792,-1445.117008,-5918.737594,6576.967875,-70.729530,-6495.475777,5967.338364,1297.233149,-8260.728734,9370.125196,-5505.393163,1555.636279 --49.997787,-2547.580420,12068.136308,-28052.676677,40280.039282,-35081.667957,9874.924100,20579.781253,-34024.375145,20153.308920,10529.292601,-35702.121913,40697.894343,-28252.039474,12129.964939,-2557.102237 -100.000003,781.414284,-2772.271926,4735.610574,-4209.323593,722.471906,2959.394584,-3288.377841,35.294844,3247.746206,-2983.638693,-648.993422,4130.425992,-4685.191554,2752.757438,-777.821092 +-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,781.414,-2772.272,4735.611,-4209.324,722.472,2959.395,-3288.378,35.295,3247.746,-2983.639,-648.993,4130.426,-4685.192,2752.757,-777.821 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv index b87aeed8ef..fa523e3cda 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.000003,-781.421166,2772.262391,-4735.692559,4209.317396,-722.558504,-2959.368797,3288.483937,-35.364765,-3247.737889,2983.669182,648.616574,-4130.364367,4685.062598,-2752.696581,777.818140 --24.998893,-1273.790210,6034.068154,-14026.338339,20140.019641,-17540.833978,4937.462050,10289.890626,-17012.187572,10076.654460,5264.646301,-17851.060956,20348.947172,-14126.019737,6064.982470,-1278.551118 -100.000002,390.703701,-1386.140730,2367.764295,-2104.664895,361.192653,1479.710185,-1644.135872,17.612462,1623.877262,-1491.804102,-324.685135,2065.243808,-2342.660255,1376.409147,-388.912022 +-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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory_inclined_circle.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory_inclined_circle.csv deleted file mode 100644 index 37abebcd1c..0000000000 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory_inclined_circle.csv +++ /dev/null @@ -1,3 +0,0 @@ --0.000004,-937.705399,3326.714870,-5682.831071,5051.180875,-867.070205,-3551.242557,3946.180725,-42.437718,-3897.285466,3580.403018,778.339889,-4956.437241,5622.075118,-3303.235898,933.381767 --29.998672,-1528.548252,7240.881785,-16831.606006,24168.023569,-21049.000774,5924.954460,12347.868752,-20414.625087,12091.985352,6317.575561,-21421.273148,24418.736606,-16951.223684,7277.978963,-1534.261342 -100.000002,468.845818,-1663.366969,2841.333551,-2525.596635,433.448504,1775.647065,-1972.984266,21.148938,1948.651051,-1790.171020,-389.546792,2478.280245,-2811.166515,1651.678805,-466.693836