public void CalculateCompensation(double dt, Plane plane, PositionLoop positionLoop, int step_count)
{
if (DeckEnable && (plane.l_path > plane.l_path_0 - 1620))
{
if (DeckCompensationStartCount < DeckCompensationStartThreshold)
{
DeckCompensationStartCount++;
}
else
{
DeckMotionCount++;
positionLoop.X1Desired[1] = positionLoop.X1Desired[1] - CurrentDeckControl[step_count];
}
// lateral deck motion, new added in mk4.1
if (DeckCompensationLateralStartCount < DeckCompensationLateralStartThreshold)
{
DeckCompensationLateralStartCount++;
}
else
{
DeckMotionLateralCount++;
positionLoop.X1Desired[0] = positionLoop.X1Desired[0] + CurrentDeckLateralControl[step_count];
}
}
switch (Configuration.TrajactoryConfig)
{
case TrajactoryType.TypeI:
vector_trac_err = HelperFunction.vector_filed_trac(plane.Position, Position);
break;
case TrajactoryType.TypeII:
vector_trac_err = HelperFunction.vector_filed_trac(plane.Position, Position);
if (DeckCompensationStartCount > (DeckCompensationStartThreshold - 1))
{
vector_trac_err[1] = vector_trac_err[1] - (-CurrentDeckControl[step_count]);
DeriveDeckControl = ((-CurrentDeckControl[step_count]) - (-CurrentDeckControl[step_count - 1])) / dt; // current_deck_control向上为正 derive_deck_control向下为正
}
else
{
DeriveDeckControl = 0;
}
// 横向运动与补偿 new added in mk4.1
if (DeckCompensationLateralStartCount > (DeckCompensationLateralStartThreshold - 1))
{
vector_trac_err[0] = vector_trac_err[0] - (CurrentDeckLateralControl[step_count]);
DeriveDeckLateralControl = ((CurrentDeckLateralControl[step_count]) - (CurrentDeckLateralControl[step_count - 1])) / dt; // 向右为正
}
else
{
DeriveDeckLateralControl = 0;
}
break;
default:
break;
}
}
public void CalculatePrescribedParameter()
{
previousX1Desired = X1Desired;
var current_desired_position = vb.Dense(3, _plane.Position[0]);
current_desired_position.SetSubVector(
1, 2, HelperFunction.ideal_path(_plane.Position, _ship.Position, _ship.Theta, _ship.Psi));
X1Desired = current_desired_position.SubVector(1, 2);
epp = X1Desired - _plane.Position.SubVector(1, 2); // 不使用滤波器得到的横向、纵向追踪误差，结果更准确 特别注意，此值用于反步法可能导致未考虑甲板运动补偿
epp /= Cos(-_ship.Theta + _ship.Psi);
}
public void Reset(Ship ship)
{
//var current_position_ship = ship.Position;
//double ship.Theta = ship.Theta;
//double ship.Psi = ship.Psi;
Alpha = 6.0 * Pi / 180; // 5.0
Miu = 0 * Pi / 180;
Beta = 0 * Pi / 180;
Phi = 0; // 欧拉角
Psi = 0;
P = 0 * Pi / 180;
Q = 0 * Pi / 180;
R = 0 * Pi / 180;
Vk = 72; // 70
DeltaA = 0 * Pi / 180;
DeltaE = 0 * Pi / 180;
DeltaR = 0 * Pi / 180;
DeltaP = 0.100; // 0.10
DeltaLEF = 33 * Pi / 180 * 1; // leading - edge flap
DeltaTEF = 45 * Pi / 180 * 1; // tailing - edge flap
GammaDerive = 0;
ChiDerive = 0;
Flow = 0.5 * PlaneInertia.Rou * Math.Pow(Vk, 2);
T = PlaneInertia.TMax * DeltaP;
CalculatePneumaticParameters();
CalculateForceAndMoment();
Initialize(ship);
Theta = Gamma + Alpha;
DesiredPosition = vb.Dense(3, Position[0]);
DesiredPosition.SetSubVector(
1, 2, HelperFunction.ideal_path(Position, ship.Position, ship.Theta, ship.Psi));
DeltaTEFDesired = DeltaTEF;
l_path = 0;
l_path_0 = 3500;
}
public Plane(Ship ship)
{
PlaneInertia = new()
{
Ixx = 74608.24,// 转动惯量 kg/m2
Iyy = 227616.4,
Izz = 231114.32,
Ixz = 0,
WingS = 62.006, // 翼面积
WingC = 4.218, // 平均气动弦长
WingL = 14.70, // 翼展
Mass = 16382.85, // 空重 kg
TMax = 122.6 * 1000, // 军用推力（两台）
Rou = 1.225, // 空气密度
G = 9.8
};
DesiredParameter = new()
{
Alpha = 10 * Pi / 180, // 期望迎角 9.1
Chi = 0 * Pi / 180, // 期望航向角
Gamma = -3.5 * Pi / 180, // 期望爬升角
Vk = 62, // 期望速度 71
EngineDelta = 0 * Pi / 180 // 发动机安装角
};
Flow = 0.5 * PlaneInertia.Rou * Math.Pow(Vk, 2);
T = PlaneInertia.TMax * DeltaP;
ThrustRange = PlaneInertia.TMax * DeltaPRange;
ThrustRateRange = PlaneInertia.TMax * DeltaPRateRange;
CalculatePneumaticParameters();
CalculateForceAndMoment();
Initialize(ship);
Theta = Gamma + Alpha;
DeltaTEFDesired = DeltaTEF;
DesiredPosition = vb.Dense(3, Position[0]);
DesiredPosition.SetSubVector(
1, 2, HelperFunction.ideal_path(Position, ship.Position, ship.Theta, ship.Psi));
}
void AddListeners()
{
}
void Initialize(Ship ship)
{
Vector <double> p_d_2p;
if (l_path_0 > 1620)
{
double x_d_2p = -1620 * Cos(ship.Theta) * Cos(ship.Gamma) - (l_path_0 - 1620); // 期望点坐标 P系下表示
double y_d_2p = 1620 * Sin(ship.Theta) * Cos(ship.Gamma);
double z_d_2p = 1620 * Sin(ship.Gamma);
p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
}
else
{
double x_d_2p = -(l_path_0 - l_path) * Cos(ship.Theta) * Cos(ship.Gamma); // 期望点坐标 P系下表示
double y_d_2p = (l_path_0 - l_path) * Sin(ship.Theta) * Cos(ship.Gamma);
double z_d_2p = (l_path_0 - l_path) * Sin(ship.Gamma);
p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
}
Matrix <double> R_i2p = mb.DenseOfArray(new double[, ] {
{ Cos(ship.Psi), Sin(ship.Psi), 0 }, { -Sin(ship.Psi), Cos(ship.Psi), 0 }, { 0, 0, 1 }
});
Matrix <double> R_p2i = R_i2p.Transpose();
Vector <double> p_d_2i = R_p2i * p_d_2p + ship.Position; // 期望点坐标 I系下表示
double kai_f = 0;
double gamma_f = 0;
Matrix <double> R_i2f = mb.DenseOfArray(new double[, ] {
{ Cos(gamma_f) * Cos(kai_f), Cos(gamma_f) * Sin(kai_f), -Sin(gamma_f) },
{ -Sin(kai_f), Cos(kai_f), 0 },
{ Sin(gamma_f) * Cos(kai_f), Sin(gamma_f) * Sin(kai_f), Cos(gamma_f) }
});
Matrix <double> R_f2i = R_i2f.Transpose();
Vector <double> p_b_2f = vb.Dense(new double[] { 0, 5, 10 }); // 飞机在F系中初始位置
x_b_2f = p_b_2f[0];
y_b_2f = p_b_2f[1];
z_b_2f = p_b_2f[2];
Position = R_f2i * p_b_2f + p_d_2i; // 飞机在I系中初始位置
Gamma = 0; // 初始爬升角
Chi = -ship.Theta; // 初始航向角
Vector <double> omega_d_2f = R_i2f * ship.omega_d_2i;
double omega_dx_2f = omega_d_2f[0];
double omega_dy_2f = omega_d_2f[1];
double omega_dz_2f = omega_d_2f[2];
Vector <double> omega_f_2f = vb.Dense(new double[] { omega_dx_2f, omega_dy_2f, omega_dz_2f });
omega_fx_2f = omega_f_2f[0];
omega_fy_2f = omega_f_2f[1];
omega_fz_2f = omega_f_2f[2];
kai_b2f = Chi - kai_f;
gamma_b2f = Gamma - gamma_f;
}
public void Record()
{
RecordPlaneStateEvent?.Invoke(this, null);
}
void CalculatePneumaticParameters()
{
double WingC = PlaneInertia.WingC;
double WingS = PlaneInertia.WingS;
double WingL = PlaneInertia.WingL;
double CM_delta_tef = 0.001 * 180 / Pi;
double CM_delta_lef = 0;
double Cnormal_delta_tef = 0.009 * 180 / Pi;
double Cnormal_delta_lef = Cnormal_delta_tef * 0.5; // 推测数据，缺少实测数据
double Caxis_delta_tef = 0;
double Caxis_delta_lef = 0;
// 升力系数相关参数 弧度制
double CY_alpha3 = -20.779;
double CY_alpha2 = 1.1682;
double CY_alpha1 = 3.8091;
double CY_0 = 0.12;
CY_alpha = CY_alpha3 * Math.Pow(Alpha, 2) + CY_alpha2 * Alpha + CY_alpha1;
double CY_delta_e = 0.6;
double CY_delta_lef = 0;
CY_delta_tef = 0.02 * 57.3;
CY = CY_0
+ CY_alpha * Alpha
+ CY_delta_e * DeltaE
+ CY_delta_lef * DeltaLEF // leading - egde flap && tailing - edge flap
+ CY_delta_tef * DeltaTEF; // lift coefficient
// 阻力系数相关参数 弧度制
double CD_alpha2 = 0.3009;
double CD_alpha1 = -0.0622;
double CD_alpha0 = -1.4994;
double CD_0 = -0.0019 + 0.0109;
CD_alpha = CD_alpha2 * Alpha + CD_alpha1;
double CD_delta_e = 0.04;
double CD_delta_lef = 0;
double CD_delta_tef = 0.002 * 57.3;
double CD_Ma;
if (Vk / 340 <= 0.81)
{
CD_Ma = 0;
}
else
{
CD_Ma = 0.1007 * Math.Pow(Vk / 340, 3) - 0.4653 * Math.Pow(Vk / 340, 2) + 0.6903 * (Vk / 340) - 0.3074;
}
double CD_beta1 = 0.4946;
double CD_beta = CD_beta1 * Beta;
CD = CD_0
+ CD_beta * Beta // 侧滑阻力
+ CD_alpha * Alpha // 迎角引起的阻力
+ CD_delta_e * Math.Abs(DeltaE) // 升降舵偏转引起的阻力。
+ CD_delta_lef * DeltaLEF // leading - egde flap引起的阻力认为是0 && tailing - edge flap
+ CD_delta_tef * DeltaTEF // 襟翼引起的阻力
+ 0.1 * Math.Pow(CY, 2) // 升致阻力
+ CD_Ma; // 马赫导致的阻力 // drag coefficient
// 侧力系数 弧度制
CC_beta = -1;
CC = CC_beta * Beta;
// 滚转力矩相关参数 弧度制
CL_beta = -0.1;
CL_delta_a = 0.12;
CL_delta_r = 0.01;
double CL_p = -0.4;
double CL_r = 0.15;
CL = CL_beta * Beta // beta引起的滚转力矩
+ CL_delta_a * DeltaA
+ CL_delta_r * DeltaR
+ WingL / 2 / Vk * CL_p * P
+ WingL / 2 / Vk * CL_r * R; // rolling moment coefficient
// 俯仰力矩相关参数 弧度制
CM_delta_e = 0.35 * (Vk / 340) - 1.1;
double CM_q = -23;
CM_alpha = -0.1;
double CM_alpha_dot = -9;
CM = CM_alpha * Alpha // 由alpha引起的俯仰力矩
+ CM_delta_e * DeltaE // 升降舵产生的俯仰力矩
+ WingC / 2 / Vk * CM_q * Q; // 俯仰角速度产生的俯仰力矩
// 偏航力矩相关参数 弧度制
CN_beta = 0.12;
CN_delta_r = -0.15;
double CN_r = -0.15;
CN_delta_a = 0;
CN = CN_beta * Beta
+ CN_delta_r * DeltaR
+ WingL / 2 / Vk * CN_r * R; // yawing moment coefficient
}
void CalculateForceAndMoment()
{
Y = Flow * PlaneInertia.WingS * CY;
D = Flow * PlaneInertia.WingS * CD;
C = Flow * PlaneInertia.WingS * CC;
L = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CL;
M = Flow * PlaneInertia.WingS * PlaneInertia.WingC * CM;
N = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CN;
}