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