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