mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-11 05:03:34 +08:00
changed PD params to 2nd order sys formulation
This commit is contained in:
Marvin Harms
@@ -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();
|
||||
|
||||
@@ -174,22 +174,21 @@ 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,
|
||||
// controller params
|
||||
(ParamFloat<px4::params::DS_K_X_ROLL>) _param_k_x_roll,
|
||||
(ParamFloat<px4::params::DS_K_X_PITCH>) _param_k_x_pitch,
|
||||
(ParamFloat<px4::params::DS_K_X_YAW>) _param_k_x_yaw,
|
||||
(ParamFloat<px4::params::DS_K_V_ROLL>) _param_k_v_roll,
|
||||
(ParamFloat<px4::params::DS_K_V_PITCH>) _param_k_v_pitch,
|
||||
(ParamFloat<px4::params::DS_K_V_YAW>) _param_k_v_yaw,
|
||||
(ParamFloat<px4::params::DS_K_A_ROLL>) _param_k_a_roll,
|
||||
(ParamFloat<px4::params::DS_K_A_PITCH>) _param_k_a_pitch,
|
||||
(ParamFloat<px4::params::DS_K_A_YAW>) _param_k_a_yaw,
|
||||
(ParamFloat<px4::params::DS_K_Q_ROLL>) _param_k_q_roll,
|
||||
(ParamFloat<px4::params::DS_K_Q_PITCH>) _param_k_q_pitch,
|
||||
(ParamFloat<px4::params::DS_K_Q_YAW>) _param_k_q_yaw,
|
||||
(ParamFloat<px4::params::DS_K_W_ROLL>) _param_k_w_roll,
|
||||
(ParamFloat<px4::params::DS_K_W_PITCH>) _param_k_w_pitch,
|
||||
(ParamFloat<px4::params::DS_K_W_YAW>) _param_k_w_yaw,
|
||||
// position INDI control params
|
||||
(ParamFloat<px4::params::DS_LIN_K_X>) _param_lin_k_x,
|
||||
(ParamFloat<px4::params::DS_LIN_K_Y>) _param_lin_k_y,
|
||||
(ParamFloat<px4::params::DS_LIN_K_Z>) _param_lin_k_z,
|
||||
(ParamFloat<px4::params::DS_LIN_C_X>) _param_lin_c_x,
|
||||
(ParamFloat<px4::params::DS_LIN_C_Y>) _param_lin_c_y,
|
||||
(ParamFloat<px4::params::DS_LIN_C_Z>) _param_lin_c_z,
|
||||
(ParamFloat<px4::params::DS_LIN_FF_X>) _param_lin_ff_x,
|
||||
(ParamFloat<px4::params::DS_LIN_FF_Y>) _param_lin_ff_y,
|
||||
(ParamFloat<px4::params::DS_LIN_FF_Z>) _param_lin_ff_z,
|
||||
// attitude INDI control params
|
||||
(ParamFloat<px4::params::DS_ROT_K_ROLL>) _param_rot_k_roll,
|
||||
(ParamFloat<px4::params::DS_ROT_K_PITCH>) _param_rot_k_pitch,
|
||||
(ParamFloat<px4::params::DS_ROT_C_ROLL>) _param_rot_c_roll,
|
||||
(ParamFloat<px4::params::DS_ROT_C_PITCH>) _param_rot_c_pitch,
|
||||
// low-level controller params
|
||||
(ParamFloat<px4::params::DS_K_ACT_ROLL>) _param_k_act_roll,
|
||||
(ParamFloat<px4::params::DS_K_ACT_PITCH>) _param_k_act_pitch,
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user