From 55992cd3c7086d300b42b551a6fe4e1e2fefc06f Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Mon, 6 Jun 2022 09:33:37 +0200 Subject: [PATCH] backup changes --- .../FixedwingPositionINDIControl.cpp | 96 ++++++++++++++++--- .../FixedwingPositionINDIControl.hpp | 21 +++- .../fw_dyn_soar_control_params.c | 46 ++++++++- .../imu_gyro_parameters.c | 2 +- 4 files changed, 143 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index a9dc33daeb..85d030ae1d 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -333,14 +333,21 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) return; } +void +FixedwingPositionINDIControl::_select_trajectory(float initial_energy) +{ + +} + void FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(string filename) { // File pointer std::ifstream fin; - // Open an existing file + // 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("/fs/microsd/trajectories/"+filename); // Read the Data from the file @@ -821,6 +828,49 @@ FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) //PX4_INFO("closest point: \t%.2f\t%.2f\t%.2f", (double)_get_position_ref(t)(0), (double)_get_position_ref(t)(1), (double)_get_position_ref(t)(2)); //PX4_INFO("closest t: %.2f", (double)t); //PX4_INFO("closest distance:%.2f", (double)sqrtf(min_dist)); + + + /* + const uint n_multistage = 10; + Vector distances_multistage; + float t_multistage = 0; + float t_ref_next = 0; // stage 2 + // compute all distances + for(uint stage=1; stage<=2; stage++){ + for(uint i=0; i<=n_multistage; i++){ + t_ref = t_ref_next + float(i-n_multistage/2.f)/(powf(float(n_multistage),stage)); + // check that t_ref is always in [0,1]: + if(t_ref<0.f){ + t_ref += 1.f; + } + else if (t_ref>1.f){ + t_ref -= 1.f; + } + Vector3f pos_ref = _get_position_ref(t_ref); + //PX4_INFO("trajectory time + point: \t%.2f\t%.2f\t%.2f\t%.2f", (double)t_ref, (double)pos_ref(0), (double)pos_ref(1), (double)pos_ref(2)); + distances_multistage(i) = (pos_ref - pos)*(pos_ref - pos); + } + // get index of smallest distance + float min_dist_multistage = distances_multistage(0); + for(uint i=1; i<=n_multistage; i++){ + if(distances_multistage(i)1.f){ + t_multistage -= 1.f; + } + } + } + // next starting point is previous closest point + t_ref_next = t_multistage; + } + PX4_INFO("different t: \t%.3f\t%.3f\t%.3f", (double)t, (double)t_multistage, (double)(t-t_multistage)); + */ + return t; } @@ -931,7 +981,21 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1)); omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2)); Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-omega_filtered) + alpha_ref; - //rot_acc_command = Vector3f{-1.0f,0.f,0.f}; + + // ========================================== + // 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}; + } + else{ + 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\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)); @@ -942,10 +1006,12 @@ Vector3f FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) { // compute the expected actuator efficiencies - float c_ail = _param_k_act_roll.get(); - float c_ele = _param_k_act_pitch.get(); - float c_rud = _param_k_act_yaw.get(); - float c_damping = 0.0f; + float k_ail = _param_k_act_roll.get(); + float k_ele = _param_k_act_pitch.get(); + float k_rud = _param_k_act_yaw.get(); + float k_d_roll = _param_k_damping_roll.get(); + float k_d_pitch = _param_k_damping_pitch.get(); + float k_d_yaw = _param_k_damping_yaw.get(); // compute velocity in body frame Dcmf R_ib(_att); Vector3f vel_body = R_ib.transpose()*_vel; @@ -953,11 +1019,16 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) //Vector3f vel_body_2 = Dcmf(Quatf(_attitude.q)).transpose()*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz}; //PX4_INFO("ENU body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body_2(0), (double)vel_body_2(1), (double)vel_body_2(2)); //PX4_INFO("FRD body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body(0), (double)vel_body(1), (double)vel_body(2)); + // filter omega at the same rate as the moments + Vector3f omega_filtered; + omega_filtered(0) = _lp_filter_omega_2[0].apply(_omega(0)); + omega_filtered(1) = _lp_filter_omega_2[1].apply(_omega(1)); + omega_filtered(2) = _lp_filter_omega_2[2].apply(_omega(2)); // compute moments Vector3f moment; - moment(0) = c_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - c_damping*q*_omega(0); - moment(1) = c_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH]; - moment(2) = c_rud*q*_actuators.control[actuator_controls_s::INDEX_YAW]; + moment(0) = k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - k_d_roll*q*omega_filtered(0); + moment(1) = k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - k_d_pitch*q*omega_filtered(1); + moment(2) = k_rud*q*_actuators.control[actuator_controls_s::INDEX_YAW] - k_d_yaw*q*omega_filtered(2); // introduce artificial time delay that is also present in acceleration Vector3f moment_filtered; moment_filtered(0) = _lp_filter_delay[0].apply(moment(0)); @@ -969,10 +1040,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered; // perform dynamic inversion Vector3f deflection; - deflection(0) = (moment_command(0)+c_damping*q*_omega(0))/(c_ail*q); - deflection(1) = moment_command(1)/(c_ele*q); - deflection(2) = moment_command(2)/(c_rud*q); - //PX4_INFO("filtered alpha: \t%.2f\t%.2f", (double)(_l_list(0))(1), (double)(_l_lpf_list(0))(1)); + deflection(0) = (moment_command(0) + k_d_roll*q*omega_filtered(0))/(k_ail*q); + deflection(1) = (moment_command(1) + k_d_pitch*q*omega_filtered(1))/(k_ele*q); + deflection(2) = (moment_command(2) + k_d_yaw*q*omega_filtered(2))/(k_rud*q); return deflection; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 20725af218..2af153e78c 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -186,9 +186,13 @@ private: (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, // location params (ParamFloat) _param_origin_lat, (ParamFloat) _param_origin_lon, @@ -233,7 +237,7 @@ private: const static size_t _num_basis_funs = 16; // number of basis functions used for the trajectory approximation // controller methods - + void _select_trajectory(float initial_energy); // select the correct trajectory based on available energy void _read_trajectory_coeffs_csv(std::string filename); // read in the correct coefficients of the appropriate trajectory void _set_wind_estimate(Vector3f wind); float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position @@ -287,13 +291,14 @@ private: // controller frequency const float _sample_frequency = 250.f; // Low-Pass filters stage 1 - const float _cutoff_frequency_1 = 2.f; + 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 - math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, 2}, {_sample_frequency, 2}, {_sample_frequency, 2}}; // force command - math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, 2}, {_sample_frequency, 2}, {_sample_frequency, 2}}; // body rates + math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command + math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates // Low-Pass filters stage 2 - const float _cutoff_frequency_2 = 15.f; + const float _cutoff_frequency_2 = 5.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF" math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // filter to match acceleration processing delay + math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // body rates uint _counter = 0; hrt_abstime _last_time{0}; @@ -334,6 +339,12 @@ private: hrt_abstime _slip_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts. float _slip{0.0f}; + // vectors defining the initial velocities, wind speed and shear strength + Vector _initial_velocities_trajectory = {}; + Vector _wind_speed_trajectory = {}; + Vector _shear_param_trajectory = {}; + + // helper variables Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame Dcmf _R_enu_to_ned; // rotation matrix from ENU to NED frame 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 37e253eb99..e75d41d951 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 @@ -426,6 +426,10 @@ PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f); */ PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f); +// ============================= +// low level INDI control params +// ============================= + /** * roll gain of K_ACT (actuator deflection gain) * @@ -436,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.3f); +PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f); /** * pitch gain of K_ACT (actuator deflection gain) @@ -448,7 +452,7 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.3f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.2f); +PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f); /** * yaw gain of K_ACT (actuator deflection gain) @@ -460,7 +464,43 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.2f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_YAW, 1.f); +PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.02f); + +/** + * roll gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f); + +/** + * pitch gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f); + +/** + * yaw gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(K_DAMPING_YAW, 0.0f); // =================================================== // ============== trajectory center ================= diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index a08e0de178..7fe237d4ab 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -121,7 +121,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 15.0f); +PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 5.0f); /** * IMU gyro dynamic notch filtering