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");
}
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;
}
}
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;
}
}
