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

increased low-level INIDIfilter frequency

Browse Source
This commit is contained in:
Marvin Harms
2022-06-28 11:44:52 +02:00
parent 0d6d749a38
commit abb09e5bba
4 changed files with 16 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+2 -32
View File
@@ -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<Vector3f, 3> &old_input, Vector<Vector3f, 2> &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);
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+1 -24
View File
@@ -174,12 +174,6 @@ private:
(ParamFloat<px4::params::DS_C_D2>) _param_fw_c_d2,
(ParamFloat<px4::params::DS_AOA_OFFSET>) _param_aoa_offset,
(ParamFloat<px4::params::DS_STALL_SPEED>) _param_stall_speed,
// filter params
(ParamFloat<px4::params::DS_FILTER_A1>) _param_filter_a1,
(ParamFloat<px4::params::DS_FILTER_A2>) _param_filter_a2,
(ParamFloat<px4::params::DS_FILTER_B1>) _param_filter_b1,
(ParamFloat<px4::params::DS_FILTER_B2>) _param_filter_b2,
(ParamFloat<px4::params::DS_FILTER_B3>) _param_filter_b3,
// controller params
(ParamFloat<px4::params::DS_K_X_ROLL>) _param_k_x_roll,
(ParamFloat<px4::params::DS_K_X_PITCH>) _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<Vector3f, 3> &old_input, Vector<Vector3f, 2> &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<Vector3f, 3> _f_list; // force
Vector<Vector3f, 3> _m_list; // moment
Vector<Vector3f, 3> _w_list; // body rates
Vector<Vector3f, 3> _a_list; // linear accel
Vector<Vector3f, 3> _l_list; // angular accel
Vector<Vector3f, 2> _f_lpf_list;
Vector<Vector3f, 2> _m_lpf_list;
Vector<Vector3f, 2> _w_lpf_list; // body rates
Vector<Vector3f, 2> _a_lpf_list;
Vector<Vector3f, 2> _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;
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+11 -72
View File
@@ -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 =================
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
+2 -2
View File
@@ -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