From 717d61e094ac28c9ec8e2394ca1145eb991b63db Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Wed, 29 Jun 2022 14:01:23 +0200 Subject: [PATCH] changed PD params to 2nd order sys formulation --- .../FixedwingPositionINDIControl.cpp | 30 ++-- .../FixedwingPositionINDIControl.hpp | 31 ++-- .../fw_dyn_soar_control_params.c | 148 ++++++++---------- 3 files changed, 93 insertions(+), 116 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 6e5df1a027..52a4230204 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -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(); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 2c4bb602d2..410b0a1312 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -174,22 +174,21 @@ private: (ParamFloat) _param_fw_c_d2, (ParamFloat) _param_aoa_offset, (ParamFloat) _param_stall_speed, - // controller params - (ParamFloat) _param_k_x_roll, - (ParamFloat) _param_k_x_pitch, - (ParamFloat) _param_k_x_yaw, - (ParamFloat) _param_k_v_roll, - (ParamFloat) _param_k_v_pitch, - (ParamFloat) _param_k_v_yaw, - (ParamFloat) _param_k_a_roll, - (ParamFloat) _param_k_a_pitch, - (ParamFloat) _param_k_a_yaw, - (ParamFloat) _param_k_q_roll, - (ParamFloat) _param_k_q_pitch, - (ParamFloat) _param_k_q_yaw, - (ParamFloat) _param_k_w_roll, - (ParamFloat) _param_k_w_pitch, - (ParamFloat) _param_k_w_yaw, + // position INDI control params + (ParamFloat) _param_lin_k_x, + (ParamFloat) _param_lin_k_y, + (ParamFloat) _param_lin_k_z, + (ParamFloat) _param_lin_c_x, + (ParamFloat) _param_lin_c_y, + (ParamFloat) _param_lin_c_z, + (ParamFloat) _param_lin_ff_x, + (ParamFloat) _param_lin_ff_y, + (ParamFloat) _param_lin_ff_z, + // attitude INDI control params + (ParamFloat) _param_rot_k_roll, + (ParamFloat) _param_rot_k_pitch, + (ParamFloat) _param_rot_c_roll, + (ParamFloat) _param_rot_c_pitch, // low-level controller params (ParamFloat) _param_k_act_roll, (ParamFloat) _param_k_act_pitch, 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 8e7c903a5a..5c06b28feb 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 @@ -198,7 +198,7 @@ PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f); // =================== CONTROL GAINS ====================== // ======================================================== /** - * roll gain of K_x (position error gain) + * control gain of position PD-controller (body x-direction) * * @unit * @min -10 @@ -207,10 +207,10 @@ PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_X_ROLL, 1.0f); +PARAM_DEFINE_FLOAT(DS_LIN_K_X, 1.0f); /** - * pitch gain of K_x (position error gain) + * control gain of position PD-controller (body y-direction) * * @unit * @min -10 @@ -219,10 +219,10 @@ PARAM_DEFINE_FLOAT(DS_K_X_ROLL, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_X_PITCH, 1.0f); +PARAM_DEFINE_FLOAT(DS_LIN_K_Y, 1.0f); /** - * yaw gain of K_x (position error gain) + * control gain of position PD-controller (body z-direction) * * @unit * @min -10 @@ -231,10 +231,46 @@ PARAM_DEFINE_FLOAT(DS_K_X_PITCH, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_X_YAW, 1.0f); +PARAM_DEFINE_FLOAT(DS_LIN_K_Z, 1.0f); /** - * roll gain of K_v (velocity error gain) + * normalized damping coefficient of position PD-controller (body x-direction) + * + * @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_C_X, 0.75f); + +/** + * normalized damping coefficient of position PD-controller (body y-direction) + * + * @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 0.75f); + +/** + * normalized damping coefficient of position PD-controller (body z-direction) + * +* @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 0.75f); + +/** + * acceleration feedback gain of position PD-controller (body x-direction) * * @unit * @min -10 @@ -243,10 +279,10 @@ PARAM_DEFINE_FLOAT(DS_K_X_YAW, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_V_ROLL, 1.5f); +PARAM_DEFINE_FLOAT(DS_LIN_FF_X, 0.5f); /** - * pitch gain of K_v (velocity error gain) + * acceleration feedback gain of position PD-controller (body y-direction) * * @unit * @min -10 @@ -255,10 +291,10 @@ PARAM_DEFINE_FLOAT(DS_K_V_ROLL, 1.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_V_PITCH, 1.5f); +PARAM_DEFINE_FLOAT(DS_LIN_FF_Y, 0.5f); /** - * yaw gain of K_v (velocity error gain) + * acceleration feedback gain of position PD-controller (body z-direction) * * @unit * @min -10 @@ -267,46 +303,10 @@ PARAM_DEFINE_FLOAT(DS_K_V_PITCH, 1.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_V_YAW, 1.5f); +PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f); /** - * roll gain of K_A (acceleration error gain) - * - * @unit - * @min -10 - * @max 10 - * @decimal 1 - * @increment 0.1 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_K_A_ROLL, 0.5f); - -/** - * pitch gain of K_A (acceleration error gain) - * - * @unit - * @min -10 - * @max 10 - * @decimal 1 - * @increment 0.1 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_K_A_PITCH, 0.5f); - -/** - * yaw gain of K_A (acceleration error gain) - * - * @unit - * @min -10 - * @max 10 - * @decimal 1 - * @increment 0.1 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_K_A_YAW, 0.5f); - -/** - * roll gain of K_Q (attitude error gain) + * control gain of attitude PD-controller (body roll-direction) * * @unit * @min 0 @@ -315,10 +315,10 @@ PARAM_DEFINE_FLOAT(DS_K_A_YAW, 0.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_Q_ROLL, 10.0f); +PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 10.0f); /** - * pitch gain of K_Q (attitude error gain) + * control gain of attitude PD-controller (body pitch-direction) * * @unit * @min 0 @@ -327,55 +327,33 @@ PARAM_DEFINE_FLOAT(DS_K_Q_ROLL, 10.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_Q_PITCH, 10.0f); +PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 10.0f); + /** - * yaw gain of K_Q (attitude error gain) + * normalized damping coefficient of attitude PD-controller (body roll-direction) * * @unit * @min 0 - * @max 20 - * @decimal 1 - * @increment 0.1 + * @max 2 + * @decimal 2 + * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_Q_YAW, 0.5f); +PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 0.75f); /** - * roll gain of K_W (angular velocity error gain) + * normalized damping coefficient of attitude PD-controller (body pitch-direction) * * @unit * @min 0 - * @max 20 - * @decimal 1 - * @increment 0.1 + * @max 2 + * @decimal 2 + * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_W_ROLL, 12.0f); +PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 0.75f); -/** - * pitch gain of K_W (angular velocity error gain) - * - * @unit - * @min 0 - * @max 20 - * @decimal 1 - * @increment 0.1 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_K_W_PITCH, 12.0f); - -/** - * yaw gain of K_W (angular velocity error gain) - * - * @unit - * @min 0 - * @max 20 - * @decimal 1 - * @increment 0.1 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(DS_K_W_YAW, 1.0f); // ============================= // low level INDI control params