public ToOrbitExecutor() : base(null) { ErrorThreshold.Lower = ToOrbitAutopilot.C.Dtol / 10; ErrorThreshold.Upper = ToOrbitAutopilot.C.Dtol; pitch.setPID(C.PitchPID); pitch.setClamp(AttitudeControlBase.C.MaxAttitudeError); throttle.setPID(C.ThrottlePID); throttle.setClamp(0.5f); norm_correction.setPID(C.NormCorrectionPID); norm_correction.setClamp(AttitudeControlBase.C.MaxAttitudeError); TimeToApA.Value = TrajectoryCalculator.C.ManeuverOffset; MinThrottle.Value = C.MinThrottle; MaxG.Value = C.MaxG; }
public void Init() { eStats = VSL.Engines.NoActiveEngines ? VSL.Engines.GetNearestEnginedStageStats() : VSL.Engines.GetEnginesStats(VSL.Engines.Active); pitch.setPID(TargetedToOrbitExecutor.C.TargetPitchPID); pitch.Min = -AttitudeControlBase.C.MaxAttitudeError; pitch.Reset(); throttle.setPID(ToOrbitExecutor.C.ThrottlePID); throttle.setClamp(0.5f); throttle.Reset(); throttle_correction.setPID(TargetedToOrbitExecutor.C.ThrottleCorrectionPID); throttle_correction.Reset(); correction_started.Reset(); }
public void SetParams(float at_clamp, float av_clamp) { atPID.setClamp(0, at_clamp); avPID.setClamp(av_clamp); }
public bool TargetedGravityTurn(float Dtol) { UpdateTargetPosition(); VSL.Engines.ActivateNextStageOnFlameout(); update_state(Dtol); var pg_vel = get_pg_vel(); if (!ErrorThreshold) { CircularizationOffset = -1; tune_THR(); var startF = getStartF(); var vel = TrajectoryCalculator.dV4ApV(VesselOrbit, target, VSL.Physics.UT); var AoA = Utils.Angle2(pg_vel, VesselOrbit.pos); var neededAoA = Utils.ClampH(Utils.Angle2(target - VesselOrbit.pos, VesselOrbit.pos) / 2, 45); var angle2Hor = angle2hor_filter.Update(90 - Utils.Angle2(vel, VesselOrbit.pos)); pitch.Max = AoA < neededAoA ? 0 : (float)AoA; pitch.Update((float)angle2Hor); correction_started.Update(angle2Hor >= 0); if (AoA < neededAoA && !correction_started) { pitch.Action = (float)Utils.Clamp(AoA - neededAoA + angle2Hor, -AttitudeControlBase.C.MaxAttitudeError, AoA); } vel = QuaternionD.AngleAxis(pitch.Action * startF, Vector3d.Cross(target, VesselOrbit.pos)) * pg_vel; if (Vector3d.Dot(vel, VesselOrbit.pos) < 0) { vel = Vector3d.Exclude(VesselOrbit.pos, vel); } vel = tune_needed_vel(vel, pg_vel, startF); throttle_correction.setClamp(Utils.ClampL(TimeToApA - 10, 1)); throttle_correction.Update((float)angle2Hor); throttle.Update(TimeToApA + throttle_correction - (float)TimeToClosestApA); throttle_filter.Update(Utils.Clamp(0.5f + throttle, 0, max_G_throttle())); THR.Throttle = throttle_filter * (float)Utils.ClampH(ErrorThreshold.Value / Dtol / 10, 1); //Log("alt {}, ApA {}, dApA {}, dArc {}, Err {}, angle2Hor {}, AoA {} < {}, startF {}, THR {}\n" + //"thr.correction {}\nthrottle {}\npitch {}", //VSL.Altitude.Absolute, VesselOrbit.ApA, dApA, dArc, ErrorThreshold, //angle2Hor, AoA, neededAoA, startF, THR.Throttle, //throttle_correction, throttle, pitch);//debug CFG.AT.OnIfNot(Attitude.Custom); ATC.SetThrustDirW(-vel.xzy); if (CFG.AT.Not(Attitude.KillRotation)) { if (AoA < 85) { CFG.BR.OnIfNot(BearingMode.Auto); BRC.ForwardDirection = htdir.xzy; } else { CFG.BR.OffIfOn(BearingMode.Auto); } } Status("Gravity turn..."); return(true); } return(coast(pg_vel)); }