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

changed turn coordination to FF-P formulation

Browse Source
This commit is contained in:
Marvin Harms
2022-07-11 16:13:17 +02:00
parent b4555c6e74
commit 794d3bf03e
3 changed files with 19 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+9 -14
View File
@@ -119,10 +119,10 @@ FixedwingPositionINDIControl::parameters_update()
_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) = _param_rot_k_yaw.get(); // rudder is controlled via turn coordination, not INDI
_K_q(2,2) = _param_rot_ff_yaw.get(); // 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) = _param_rot_c_yaw.get()*2.f*sqrtf(_param_rot_k_yaw.get()); // rudder is controlled via turn coordination, not INDI
_K_w(2,2) = _param_rot_p_yaw.get(); // rudder is controlled via turn coordination, not INDI
// aircraft parameters
_inertia(0,0) = _param_fw_inertia_roll.get();
@@ -139,14 +139,11 @@ FixedwingPositionINDIControl::parameters_update()
_aoa_offset = _param_aoa_offset.get();
_stall_speed = _param_stall_speed.get();
// actuator gains
_k_ail = _param_k_act_roll.get();
_k_ele = _param_k_act_pitch.get();
_k_rud = _param_k_act_yaw.get();
_k_d_roll = _param_k_damping_roll.get();
_k_d_pitch = _param_k_damping_pitch.get();
_k_d_yaw = _param_k_damping_yaw.get();
// trajectory origin
_origin_lat = _param_origin_lat.get();
@@ -776,8 +773,8 @@ FixedwingPositionINDIControl::Run()
// ====================
// publish debug values
// ====================
Vector3f vel_body = R_bi*_vel;
_slip = atan2f(vel_body(1), vel_body(0));
Vector3f vel_body = R_bi*(_vel - _wind_estimate);
_slip = atan2f(vel_body(1), vel_body(0))*180.f/M_PI_2_F;
_debug_value.timestamp = hrt_absolute_time();
_debug_value.value = _slip;
_debug_value_pub.publish(_debug_value);
@@ -1240,10 +1237,10 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
}
// transform rate vector to body frame
//omega_turn_ref = R_bi*omega_turn_ref;
float scaler = (_stall_speed*_stall_speed)/fmaxf(sqrtf(vel_body*vel_body)*vel_body(0), _stall_speed*_stall_speed);
// not really an accel command, rather a FF-P command
rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2)*scaler + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2))* scaler*scaler;
// not really a accel command, rather a FF-P command
rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2) + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2));
//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));
@@ -1269,7 +1266,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
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(2) = _k_rud*q*_actuators.control[actuator_controls_s::INDEX_YAW] - _k_d_yaw*q*omega_filtered(2);
moment(2) = 0.f;
// introduce artificial time delay that is also present in acceleration
Vector3f moment_filtered;
moment_filtered(0) = _lp_filter_delay[0].apply(moment(0));
@@ -1282,10 +1279,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
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(2) = (moment_command(2) + _k_d_yaw*q*omega_filtered(2))/fmaxf((_k_rud*q),0.0001f);
// overwrite rudder deflection with NDI turn coordination (no INDI)
Vector3f moment_ref = _inertia*ctrl + omega_filtered.cross(_inertia*omega_filtered);
deflection(2) = (moment_ref(2) + _k_d_yaw*q*omega_filtered(2)) / fmaxf((_k_rud*q),0.0001f);
deflection(2) = ctrl(2);
return deflection;
}
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+2 -6
View File
@@ -190,17 +190,15 @@ private:
// attitude PD 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_K_YAW>) _param_rot_k_yaw,
(ParamFloat<px4::params::DS_ROT_FF_YAW>) _param_rot_ff_yaw,
(ParamFloat<px4::params::DS_ROT_C_ROLL>) _param_rot_c_roll,
(ParamFloat<px4::params::DS_ROT_C_PITCH>) _param_rot_c_pitch,
(ParamFloat<px4::params::DS_ROT_C_YAW>) _param_rot_c_yaw,
(ParamFloat<px4::params::DS_ROT_P_YAW>) _param_rot_p_yaw,
// low-level controller params (INDI)
(ParamFloat<px4::params::DS_K_ACT_ROLL>) _param_k_act_roll,
(ParamFloat<px4::params::DS_K_ACT_PITCH>) _param_k_act_pitch,
(ParamFloat<px4::params::DS_K_ACT_YAW>) _param_k_act_yaw,
(ParamFloat<px4::params::DS_K_DAMP_ROLL>) _param_k_damping_roll,
(ParamFloat<px4::params::DS_K_DAMP_PITCH>) _param_k_damping_pitch,
(ParamFloat<px4::params::DS_K_DAMP_YAW>) _param_k_damping_yaw,
// location params
(ParamFloat<px4::params::DS_ORIGIN_LAT>) _param_origin_lat,
(ParamFloat<px4::params::DS_ORIGIN_LON>) _param_origin_lon,
@@ -293,10 +291,8 @@ private:
Vector3f _alpha; // angular acceleration vector
float _k_ail;
float _k_ele;
float _k_rud;
float _k_d_roll;
float _k_d_pitch;
float _k_d_yaw;
hrt_abstime _last_run{0};
// controller frequency
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+8 -30
View File
@@ -310,7 +310,7 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f);
*
* @unit
* @min 0
* @max 20
* @max 100
* @decimal 1
* @increment 0.1
* @group FW DYN SOAR Control
@@ -322,7 +322,7 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 10.0f);
*
* @unit
* @min 0
* @max 20
* @max 100
* @decimal 1
* @increment 0.1
* @group FW DYN SOAR Control
@@ -330,16 +330,16 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 10.0f);
PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 10.0f);
/**
* rudder turn coordination P-gain
* rudder turn coordination FF-gain
*
* @unit
* @min 0
* @max 20
* @max 100
* @decimal 1
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_ROT_K_YAW, 0.5f);
PARAM_DEFINE_FLOAT(DS_ROT_FF_YAW, 1.0f);
/**
* normalized damping coefficient of attitude PD-controller (body roll-direction)
@@ -366,16 +366,16 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 1.0f);
PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 1.0f);
/**
* normalized damping coefficient of rudder (turn coordination)
* rudder turn coordination P-gain
*
* @unit
* @min 0
* @max 2
* @max 100
* @decimal 2
* @increment 0.01
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_ROT_C_YAW, 1.0f);
PARAM_DEFINE_FLOAT(DS_ROT_P_YAW, 1.0f);
// =============================
@@ -406,17 +406,6 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.25f);
*/
PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f);
/**
* yaw gain of K_ACT (actuator deflection gain)
*
* @unit
* @min 0
* @max 100
* @decimal 4
* @increment 0.0001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.01f);
/**
* roll gain of K_ACT_DAMPING (actuator damping gain)
@@ -442,17 +431,6 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.04f);
*/
PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f);
/**
* yaw gain of K_ACT_DAMPING (actuator damping gain)
*
* @unit
* @min 0
* @max 100
* @decimal 4
* @increment 0.0001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f);
// ===================================================
// ============== trajectory center =================