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

changed filtering in high-levle indi

Browse Source
This commit is contained in:
Marvin Harms
2022-07-16 18:54:42 +02:00
parent 4f81d45201
commit 66e21bad47
2 changed files with 24 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+21 -12
View File
@@ -450,8 +450,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");
@@ -1125,16 +1125,20 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
{
Dcmf R_ib(_att);
Dcmf R_bi(R_ib.transpose());
// apply LP filter to acceleration
// apply LP filter to acceleration & velocity
Vector3f acc_filtered;
acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0));
acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1));
acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2));
Vector3f omega_filtered;
omega_filtered(0) = _lp_filter_omega[0].apply(_omega(0));
omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1));
omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2));
// =========================================
// apply PD control law on the body position
// =========================================
Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-acc_filtered)) + acc_ref;
Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-_acc)) + acc_ref;
// add gravity
//acc_command(2) += 9.81f;
//acc_filtered(2) += 9.81f;
@@ -1211,10 +1215,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
// =========================================
// apply PD control law on the body attitude
// =========================================
Vector3f omega_filtered;
omega_filtered(0) = _lp_filter_omega[0].apply(_omega(0));
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;
// ==========================================
@@ -1301,6 +1301,15 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
//PX4_INFO("omega turn ref: \t%.2f\t%.2f\t%.2f", (double)omega_turn_ref(0), (double)omega_turn_ref(1), (double)omega_turn_ref(2));
//PX4_INFO("omega turn : \t%.2f\t%.2f\t%.2f", (double)_omega(0), (double)_omega(1), (double)_omega(2));
// =======================================================================================
// filter the stage 1 controller outputs to filter out high-frequency components.
// This is desirable as the provided commands might be very noisy otherwise (not feasible)
// =======================================================================================
rot_acc_command(0) = _lp_filter_ctrl1[0].apply(rot_acc_command(0));
rot_acc_command(1) = _lp_filter_ctrl1[1].apply(rot_acc_command(1));
rot_acc_command(2) = _lp_filter_ctrl1[2].apply(rot_acc_command(2));
return rot_acc_command;
}
@@ -1321,8 +1330,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
omega_filtered(2) = _lp_filter_omega_2[2].apply(_omega(2));
// compute moments
Vector3f moment;
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(0) = _k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - _k_d_roll*q*_omega(0);
moment(1) = _k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - _k_d_pitch*q*_omega(1);
moment(2) = 0.f;
// introduce artificial time delay that is also present in acceleration
Vector3f moment_filtered;
@@ -1334,8 +1343,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered;
// perform dynamic inversion
Vector3f deflection;
deflection(0) = (moment_command(0) + _k_d_roll*q*omega_filtered(0))/fmaxf((_k_ail*q),0.0001f);
deflection(1) = (moment_command(1) + _k_d_pitch*q*omega_filtered(1))/fmaxf((_k_ele*q),0.0001f);
deflection(0) = (moment_command(0) + _k_d_roll*q*_omega(0))/fmaxf((_k_ail*q),0.0001f);
deflection(1) = (moment_command(1) + _k_d_pitch*q*_omega(1))/fmaxf((_k_ele*q),0.0001f);
// overwrite rudder deflection with NDI turn coordination (no INDI)
deflection(2) = ctrl(2);
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+3
View File
@@ -304,6 +304,9 @@ private:
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, _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
// smoothing filter to reject HF noise in control output
const float _cutoff_frequency_smoothing = 30.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
math::LowPassFilter2p _lp_filter_ctrl1[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}}; // control output stage 1
// Low-Pass filters stage 2
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