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

fixed error in high-level INDI

Browse Source
This commit is contained in:
Marvin Harms
2022-07-09 18:06:29 +02:00
parent 6b7d42eef5
commit b4555c6e74
3 changed files with 50 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+25 -11
View File
@@ -171,6 +171,8 @@ FixedwingPositionINDIControl::parameters_update()
_switch_manual = _param_switch_manual.get();
_switch_saturation = _param_switch_saturation.get();
// sanity check parameters
// TODO: include sanity check
@@ -1093,7 +1095,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
// =========================================
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;
// add gravity
acc_command(2) += 9.81f;
//acc_command(2) += 9.81f;
//acc_filtered(2) += 9.81f;
// ==================================
// compute expected aerodynamic force
@@ -1106,17 +1109,28 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
float factor = -0.5f*_rho*_area*sqrtf(vel_body*vel_body);
Vector3f w_x = vel_body;
Vector3f w_z = w_x.cross(Vector3f{0.f,1.f,0.f});
f_current = R_ib*(factor*(C_l*w_z + C_d*w_x)) + Vector3f{0.f,0.f,-_mass*9.81f};
f_current = R_ib*(factor*(C_l*w_z + C_d*w_x));
// apply LP filter to force
Vector3f f_current_filtered;
f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0));
f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1));
f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2));
// ===============================
// get force comand in world frame
// ===============================
// ================================
// get force command in world frame
// ================================
Vector3f f_command = _mass*(acc_command - acc_filtered) + f_current_filtered;
// limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 12° aoa)
//PX4_INFO("force current, command: \t%.2f\t%.2f", (double)sqrtf(f_current_filtered*f_current_filtered), (double)sqrtf(f_command*f_command));
if (_switch_saturation){
float f_max = -factor*sqrtf(vel_body*vel_body)*(_C_L0 + _C_L1*0.2f);
float f_now = sqrtf(f_command*f_command);
if (f_now>f_max){
f_command = f_max/f_now * f_command;
}
}
// ==========================================================================
// get required attitude (assuming we can fly the target velocity), and error
@@ -1210,10 +1224,9 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
// ==============================================================
Vector3f vel_air = _vel - _wind_estimate;
Vector3f vel_normalized = vel_air.normalized();
Vector3f f = _mass*(_acc);
Vector3f f_normalized = f.normalized();
Vector3f acc_normalized = _acc.normalized();
// compute ideal angular velocity
Vector3f omega_turn_ref_normalized = vel_normalized.cross(f_normalized);
Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized);
Vector3f omega_turn_ref;
// constuct acc perpendicular to flight path
Vector3f acc_perp = _acc - (_acc*vel_normalized)*vel_normalized;
@@ -1227,10 +1240,12 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
}
// transform rate vector to body frame
omega_turn_ref = R_bi*omega_turn_ref;
//omega_turn_ref = R_bi*omega_turn_ref;
// 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));
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));
return rot_acc_command;
}
@@ -1268,7 +1283,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
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);
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+7 -1
View File
@@ -210,7 +210,9 @@ private:
// thrust params
(ParamFloat<px4::params::DS_THRUST>) _param_thrust,
// RC feedthrough params
(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual
(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual,
// force saturation
(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation
)
@@ -340,6 +342,8 @@ private:
float _thrust;
// controller mode
bool _switch_manual;
// force limit
bool _switch_saturation;
bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists
hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts.
@@ -363,6 +367,8 @@ private:
Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame
Dcmf _R_enu_to_ned; // rotation matrix from ENU to NED frame
Vector3f _zero_crossing_local_pos; // vector denoting the zero crossing of the trajectories in NED frame
Vector3f _f_command_filtered {};
Vector3f _m_command_filtered {};
};
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+18 -2
View File
@@ -184,7 +184,7 @@ PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f);
/**
* stall speed of the aircraft
*
* @unit rad
* @unit m/s
* @min 5
* @max 10
* @decimal 1
@@ -539,4 +539,20 @@ PARAM_DEFINE_FLOAT(DS_THRUST, 0);
* @increment 1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0);
PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0);
// ======================================================
// ============= controller force saturation ============
// ======================================================
/**
* integer in {0,1} defining if the commanded force has an upper bound (saturates), 0=no saturation, 1=saturation
*
* @unit
* @min 0
* @max 1
* @decimal 1
* @increment 1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_INT32(DS_SWITCH_SAT, 0);