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

changed PD params to 2nd order sys formulation

Browse Source
This commit is contained in:
Marvin Harms
2022-06-29 14:01:23 +02:00
parent 401ccc054d
commit 717d61e094
3 changed files with 93 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+15 -15
View File
@@ -108,21 +108,21 @@ FixedwingPositionINDIControl::parameters_update()
_K_a *= 0.f;
_K_q *= 0.f;
_K_w *= 0.f;
_K_x(0,0) = _param_k_x_roll.get();
_K_x(1,1) = _param_k_x_pitch.get();
_K_x(2,2) = _param_k_x_yaw.get();
_K_v(0,0) = _param_k_v_roll.get();
_K_v(1,1) = _param_k_v_pitch.get();
_K_v(2,2) = _param_k_v_yaw.get();
_K_a(0,0) = _param_k_a_roll.get();
_K_a(1,1) = _param_k_a_pitch.get();
_K_a(2,2) = _param_k_a_yaw.get();
_K_q(0,0) = _param_k_q_roll.get();
_K_q(1,1) = _param_k_q_pitch.get();
_K_q(2,2) = _param_k_q_yaw.get();
_K_w(0,0) = _param_k_w_roll.get();
_K_w(1,1) = _param_k_w_pitch.get();
_K_w(2,2) = _param_k_w_yaw.get();
_K_x(0,0) = _param_lin_k_x.get();
_K_x(1,1) = _param_lin_k_y.get();
_K_x(2,2) = _param_lin_k_z.get();
_K_v(0,0) = _param_lin_c_x.get()*2.f*sqrtf(_param_lin_k_x.get());
_K_v(1,1) = _param_lin_c_y.get()*2.f*sqrtf(_param_lin_k_y.get());
_K_v(2,2) = _param_lin_c_z.get()*2.f*sqrtf(_param_lin_k_z.get());
_K_a(0,0) = _param_lin_ff_x.get();
_K_a(1,1) = _param_lin_ff_y.get();
_K_a(2,2) = _param_lin_ff_z.get();
_K_q(0,0) = _param_rot_k_roll.get();
_K_q(1,1) = _param_rot_k_pitch.get();
_K_q(2,2) = 0.f; // rudder is controlled via turn coordination, not INDI
_K_w(0,0) = _param_rot_c_roll.get()*2.f*sqrtf(_param_rot_k_roll.get());
_K_w(1,1) = _param_rot_c_pitch.get()*2.f*sqrtf(_param_rot_k_pitch.get());
_K_w(2,2) = 0.f; // rudder is controlled via turn coordination, not INDI
// aircraft parameters
_inertia(0,0) = _param_fw_inertia_roll.get();