public void UpdateState(double dt, Disturbance disturbance)
{
// plane.current_v = [Sin(plane.current_miu), plane.current_alpha * Cos(plane.current_miu)]';
//dt = e.data{ 1};
//current_X3_dot = e.data{ 2};
//current_X3 = current_X3 + current_X3_dot * dt;
}
public Simulation()
{
InitializeInitialParameters();
Ship = new Ship();
Plane = new Plane(Ship);
Disturbance = new Disturbance();
PositionLoop = new PositionLoop(Plane, Ship);
FlightPathLoop = new FlightPathLoop(Plane, Ship);
AttitudeLoop = new AttitudeLoop(Plane, Ship);
AngularRateLoop = new AngularRateLoop(Plane, Ship);
Record = new SimulationRecord();
Plane.X1ChangedEvent += PositionLoop.OnUpdateState;
Plane.X2ChangedEvent += FlightPathLoop.OnUpdateState;
Plane.X3ChangedEvent += AttitudeLoop.OnUpdateState;
Plane.X4ChangedEvent += AngularRateLoop.OnUpdateState;
Plane.RecordPlaneStateEvent += Record.OnRecordPlaneState;
DataSendTimer = new Timer(100);
DataSendTimer.Elapsed += (sender, e) =>
{
if (DataQueue.TryDequeue(out double data))
{
connection.InvokeAsync("SendData", "user", data);
}
if (DataQueue.IsEmpty)
{
DataSendTimer.Stop();
Console.WriteLine("Timer Stoped");
}
};
DataQueue = new ConcurrentQueue <double>();
Console.WriteLine("text");
}
void Reset()
{
step_count = 0;
current_time = 0;
Ship.reset();
Plane.Reset(Ship);
PositionLoop.Reset();
FlightPathLoop.Reset();
AttitudeLoop.Reset();
AngularRateLoop.Reset();
Disturbance.reset();
}
void SingleStep()
{
step_count++;
current_time = dt * step_count;
PositionLoop.CalculateObservation();
FlightPathLoop.CalculateObservation();
AttitudeLoop.CalculateObservation();
AngularRateLoop.CalculateObservation();
FlightPathLoop.CalculateNonlinearObserver(dt, Disturbance);
AngularRateLoop.CalculateNonlinearObserver(dt, Disturbance);
PositionLoop.CalculatePrescribedParameter();
Ship.CalculateCompensation(dt, Plane, PositionLoop, step_count);
PositionLoop.CalculateState(dt, null);
PositionLoop.CalculateOutput(dt, current_time, step_count);
PositionLoop.CalculateLimiter(dt);
FlightPathLoop.CalculateState(dt, PositionLoop.U1);
FlightPathLoop.CalculateOutput();
FlightPathLoop.CalculateLimiter(dt);
FlightPathLoop.CalculateFilter(dt);
AttitudeLoop.CalculateState(dt, FlightPathLoop.U2, FlightPathLoop.DeriveX2[1]);
AttitudeLoop.CalculateOutput();
AttitudeLoop.CalculateLimiter(dt);
AngularRateLoop.CalculateState(dt, AttitudeLoop.U3);
AngularRateLoop.CalculateOutput(dt, current_time, step_count);
AngularRateLoop.CalculateLimiter(dt);
AngularRateLoop.CalculateFilter(dt);
Plane.UpdateState(dt, Disturbance);
Ship.UpdateState(dt);
Disturbance.updateWind(Plane, Ship, step_count);
Plane.Record();
Ship.record();
PositionLoop.Record(dt);
FlightPathLoop.Record(dt);
AttitudeLoop.Record(dt);
AngularRateLoop.Record(dt);
Record.time_record.Add(current_time);
if (Plane.l_path > 1000)
{
landing_gear = true;
tail_hook = true;
}
}
public void CalculateNonlinearObserver(double dt, Disturbance disturbance)
{
return;
}
public void UpdateState(double dt, Disturbance disturbance)
{
CalculatePneumaticParameters();
CalculateForceAndMoment();
disturbance.updateState(this);
double Ixx = PlaneInertia.Ixx;
double Iyy = PlaneInertia.Iyy;
double Izz = PlaneInertia.Izz;
double Ixz = PlaneInertia.Ixz;
double Mass = PlaneInertia.Mass;
double TMax = PlaneInertia.TMax;
double Rou = PlaneInertia.Rou;
double G = PlaneInertia.G;
double AlphaDesired = DesiredParameter.Alpha; // 期望迎角 9.1
double ChiDesired = DesiredParameter.Chi; // 期望航向角
double GammaDesired = DesiredParameter.Gamma; // 期望爬升角
double VkDesired = DesiredParameter.Vk; // 期望速度 71
double EngineDelta = DesiredParameter.EngineDelta; // 发动机安装角
double current_p_dot = 1 / (Ixx * Izz - Math.Pow(Ixz, 2)) * ((Iyy * Izz - Math.Pow(Izz, 2) - Math.Pow(Ixz, 2)) * Q * R
+ (Ixx * Ixz + Izz * Ixz - Iyy * Ixz) * P * Q
+ Izz * L
+ Ixz * N)
+ disturbance.disturbance_p;
double current_q_dot = 1 / Iyy * ((Izz - Ixx) * P * R - Ixz * Math.Pow(P, 2)
+ Ixz * Math.Pow(R, 2) + M)
+ disturbance.disturbance_q;
double current_r_dot = 1 / (Ixx * Izz - Math.Pow(Ixz, 2)) * ((Math.Pow(Ixx, 2) + Math.Pow(Ixz, 2) - Ixx * Iyy) * P * Q
+ (Iyy * Ixz - Ixx * Ixz - Izz * Ixz) * Q * R
+ Ixz * L
+ Ixx * N)
+ disturbance.disturbance_r;
Vector <double> current_X4_dot = vb.Dense(new[] { current_p_dot, current_q_dot, current_r_dot });
P += current_p_dot * dt;
Q += current_q_dot * dt;
R += current_r_dot * dt;
X4ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X4_dot));
// 欧拉角
double current_phi_dot = P + Tan(Theta) * (Q * Sin(Phi) + R * Cos(Phi));
double current_theta_dot = Q * Cos(Phi) - R * Sin(Phi);
double current_psi_dot = 1 / Cos(Theta) * (Q * Sin(Phi) + R * Cos(Phi));
Phi += current_phi_dot * dt;
Theta += current_theta_dot * dt;
Psi += current_psi_dot * dt;
// 绕质心运动学方程
double current_beta_dot = P * Sin(Alpha) - R * Cos(Alpha) - GammaDerive * Sin(Miu) + ChiDerive * Cos(Miu) * Cos(Gamma);
double current_miu_dot = (P * Cos(Alpha) + R * Sin(Alpha) + GammaDerive * Sin(Beta) * Cos(Miu) + ChiDerive * (Sin(Gamma)
* Cos(Beta) + Sin(Beta) * Sin(Miu) * Cos(Gamma))) / Cos(Beta);
double current_alpha_dot = (Q * Cos(Beta) - (P * Cos(Alpha) + R * Sin(Alpha)) * Sin(Beta) - GammaDerive * Cos(Miu)
- ChiDerive * Sin(Miu) * Cos(Gamma)) / Cos(Beta);
Vector <double> current_X3_dot = vb.Dense(new[] { current_theta_dot, current_beta_dot, current_miu_dot });
// update state parameters
Alpha += current_alpha_dot * dt;
Beta += current_beta_dot * dt;
Miu += current_miu_dot * dt;
X3ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X3_dot));
// 质心动力方程
double current_kai_dot = 1 / (Mass * Vk * Cos(Gamma))
* (T * (Sin(Alpha + EngineDelta) * Sin(Miu)
- Cos(Alpha + EngineDelta)
* Sin(Beta) * Cos(Miu))
+ C * Cos(Miu)
+ Y * Sin(Miu))
+ disturbance.disturbance_kai;
double current_gamma_dot = -1 / (Mass * Vk)
* (T * (-Sin(Alpha + EngineDelta) * Cos(Miu)
- Cos(Alpha + EngineDelta) * Sin(Beta) * Sin(Miu))
+ C * Sin(Miu)
- Y * Cos(Miu)
+ Mass * G * Cos(Gamma))
+ disturbance.disturbance_gamma;
Vector <double> current_X2_dot = vb.Dense(new[] { current_kai_dot, current_gamma_dot });
double current_Vk_dot = 1 / Mass * (T * Cos(Alpha + EngineDelta) * Cos(Beta)
- D
- Mass * G * Sin(Gamma))
+ disturbance.disturbance_Vk;
// update state parameters
Chi += current_kai_dot * dt;
Gamma += current_gamma_dot * dt;
DeltaP = T / TMax;
Vk += current_Vk_dot * dt;
Flow = 0.5 * Rou * Math.Pow(Vk, 2);
GammaDerive = current_gamma_dot;
ChiDerive = current_kai_dot;
X2ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X2_dot));
// 质心运动方程
double current_x_dot = Vk * Cos(Gamma) * Cos(Chi);
double current_y_dot = Vk * Cos(Gamma) * Sin(Chi);
double current_z_dot = -Vk *Sin(Gamma);
Vector <double> current_X1_dot = vb.Dense(new[] { current_y_dot, current_z_dot });
// update state parameters
Position[0] += current_x_dot * dt;
Position[1] += current_y_dot * dt;
Position[2] += current_z_dot * dt;
X1ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X1_dot));
}
public void UpdateState(double dt, Disturbance disturbance)
{
//dt = e.data{ 1};
//current_X1_dot = e.data{ 2};
//current_X1 = current_X1 + current_X1_dot * dt;
}
public void CalculateNonlinearObserver(double dt, Disturbance disturbance)
{
throw new NotImplementedException();
}
