diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 2f35e7d190..5008c53cd4 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -139,12 +139,6 @@ FixedwingPositionINDIControl::parameters_update() _aoa_offset = _param_aoa_offset.get(); _stall_speed = _param_stall_speed.get(); - // filter parameters - _a1 = _param_filter_a1.get(); - _a2 = _param_filter_a2.get(); - _b1 = _param_filter_b1.get(); - _b2 = _param_filter_b2.get(); - _b3 = _param_filter_b3.get(); // actuator gains _k_ail = _param_k_act_roll.get(); @@ -1252,30 +1246,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) return deflection; } -/* -Vector3f -FixedwingPositionINDIControl::_apply_LP_filter(Vector3f new_input, Vector &old_input, Vector &old_output) -{ - // apply as: moment_filtered = _apply_LP_filter(moment, _m_list, _m_lpf_list); - old_input(0) = old_input(1); - old_input(1) = old_input(2); - old_input(2) = new_input; - // - Vector3f output = Vector3f{0.f,0.f,0.f}; - // - output += _a1*old_output(1); - output += _a2*old_output(0); - // - output += _b1*old_input(2); - output += _b2*old_input(1); - output += _b3*old_input(0); - // - old_output(0) = old_output(1); - old_output(1) = output; - // - return output; -} -*/ Vector3f FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl) @@ -1293,8 +1263,8 @@ FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl) float current_ele = _actuators.control[actuator_controls_s::INDEX_PITCH]; float current_rud = _actuators.control[actuator_controls_s::INDEX_YAW]; // - float max_rate = M_PI_F/2.f; - float dt = 1.f/50.f; + float max_rate = 0.5f/0.18f; // + float dt = 1.f/_sample_frequency; // deflection(0) = constrain(deflection(0),current_ail-dt*max_rate,current_ail+dt*max_rate); deflection(1) = constrain(deflection(1),current_ele-dt*max_rate,current_ele+dt*max_rate); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 86ab2908fb..92c5eff683 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -174,12 +174,6 @@ private: (ParamFloat) _param_fw_c_d2, (ParamFloat) _param_aoa_offset, (ParamFloat) _param_stall_speed, - // filter params - (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, @@ -269,7 +263,6 @@ private: Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref); Vector3f _compute_INDI_stage_2(Vector3f ctrl); - Vector3f _apply_LP_filter(Vector3f new_input, Vector &old_input, Vector &old_output); Vector3f _compute_actuator_deflections(Vector3f ctrl); // yaw controller @@ -300,17 +293,6 @@ private: float _k_d_yaw; hrt_abstime _last_run{0}; - // filter variables - Vector _f_list; // force - Vector _m_list; // moment - Vector _w_list; // body rates - Vector _a_list; // linear accel - Vector _l_list; // angular accel - Vector _f_lpf_list; - Vector _m_lpf_list; - Vector _w_lpf_list; // body rates - Vector _a_lpf_list; - Vector _l_lpf_list; // controller frequency const float _sample_frequency = 200.f; // Low-Pass filters stage 1 @@ -319,7 +301,7 @@ private: 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 = 5.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF" + const float _cutoff_frequency_2 = 30.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 // Low-Pass filter for wind estimate @@ -340,11 +322,6 @@ private: float _C_D2; float _aoa_offset; float _stall_speed; - float _a1; - float _a2; - float _b1; - float _b2; - float _b3; // trajecotry origin in WGS84 float _origin_lat; float _origin_lon; 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 7f7907a62d..15f184800c 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 @@ -52,8 +52,6 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f); */ PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f); - - /** * total takeoff mass * @@ -181,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.01f); +PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.03f); /** * stall speed of the aircraft @@ -193,67 +191,8 @@ PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.01f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 7.0f); +PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f); -/** - * coefficients of the butterworth filter used for smoothing the IMU - * - * @unit - * @min -100 - * @max 100 - * @decimal 7 - * @increment 0.0000001 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_FILTER_A1, 1.16826067f); - -/** - * coefficients of the butterworth filter used for smoothing the IMU - * - * @unit - * @min -100 - * @max 100 - * @decimal 7 - * @increment 0.0000001 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_FILTER_A2, -0.42411821f); - -/** - * coefficients of the butterworth filter used for smoothing the IMU - * - * @unit - * @min -100 - * @max 100 - * @decimal 7 - * @increment 0.0000001 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_FILTER_B1, 0.06396438f); - -/** - * coefficients of the butterworth filter used for smoothing the IMU - * - * @unit - * @min -100 - * @max 100 - * @decimal 7 - * @increment 0.0000001 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_FILTER_B2, 0.12792877f); - -/** - * coefficients of the butterworth filter used for smoothing the IMU - * - * @unit - * @min -100 - * @max 100 - * @decimal 7 - * @increment 0.0000001 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_FILTER_B3, 0.06396438f); // ======================================================== // =================== CONTROL GAINS ====================== @@ -412,7 +351,7 @@ PARAM_DEFINE_FLOAT(DS_K_Q_YAW, 0.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_W_ROLL, 5.0f); +PARAM_DEFINE_FLOAT(DS_K_W_ROLL, 12.0f); /** * pitch gain of K_W (angular velocity error gain) @@ -424,7 +363,7 @@ PARAM_DEFINE_FLOAT(DS_K_W_ROLL, 5.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_W_PITCH, 5.0f); +PARAM_DEFINE_FLOAT(DS_K_W_PITCH, 12.0f); /** * yaw gain of K_W (angular velocity error gain) @@ -452,7 +391,7 @@ PARAM_DEFINE_FLOAT(DS_K_W_YAW, 1.0f); * @increment 0.001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.1f); +PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.2f); /** * pitch gain of K_ACT (actuator deflection gain) @@ -464,7 +403,7 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.1f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.03f); +PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f); /** * yaw gain of K_ACT (actuator deflection gain) @@ -488,7 +427,7 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f); * @increment 0.001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f); +PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.04f); /** * pitch gain of K_ACT_DAMPING (actuator damping gain) @@ -500,7 +439,7 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f); * @increment 0.001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.01f); +PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f); /** * yaw gain of K_ACT_DAMPING (actuator damping gain) @@ -528,7 +467,7 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.4040182f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f); /** * longitude of trajectory start point (WGS84) @@ -540,7 +479,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.4040182f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.5101919f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8100000f); /** * altitude of trajectory start point (WGS84) @@ -552,7 +491,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.5101919f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 540.0f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f); // ====================================================== // ============== loiter circle number ================= 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 4b9fbc383b..977ca17019 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 10.0f); +PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 60.0f); /** * Gyro control data maximum publication rate @@ -121,7 +121,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 5.0f); +PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); /** * IMU gyro dynamic notch filtering