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));
}
protected override void Update()
{
if (landing)
{
do_land(); return;
}
switch (stage)
{
case Stage.Start:
VSC_ON(HFlight.Stop);
if (VSL.LandedOrSplashed || VSL.Altitude.Relative < StartAltitude)
{
CFG.DesiredAltitude = StartAltitude;
}
else
{
CFG.DesiredAltitude = VSL.Altitude.Relative;
}
if (!VSL.LandedOrSplashed)
{
compute_initial_trajectory();
}
break;
case Stage.GainAltitude:
Status("Gaining altitude...");
VSC_ON(HFlight.Level);
if (VSL.Altitude.Relative > CFG.DesiredAltitude - 10)
{
compute_initial_trajectory();
}
break;
case Stage.Compute:
double obstacle;
if (!trajectory_computed())
{
break;
}
if (find_biggest_obstacle_ahead(BJ.StartAltitude, out obstacle))
{
break;
}
if (obstacle > 0)
{
StartAltitude += (float)obstacle + BJ.ObstacleOffset;
CFG.DesiredAltitude = StartAltitude;
stage = Stage.GainAltitude;
}
else if (VSL.Altitude.Relative < BJ.StartAltitude - 10)
{
stage = Stage.GainAltitude;
}
else if (check_initial_trajectory())
{
accelerate();
}
else
{
stage = Stage.Wait;
}
break;
case Stage.Wait:
if (!string.IsNullOrEmpty(TCAGui.StatusMessage))
{
break;
}
accelerate();
break;
case Stage.Accelerate:
CFG.AT.OnIfNot(Attitude.Custom);
var dV = (trajectory.Orbit.GetFrameVelAtUT(trajectory.StartUT) -
VSL.orbit.GetFrameVelAtUT(trajectory.StartUT)).xzy;
if (VSL.Controls.AlignmentFactor > 0.9 || !CFG.VSCIsActive)
{
CFG.DisableVSC();
var dVv = -VSL.Altitude.Absolute;//Vector3d.Dot(dV, VSL.Physics.Up);
if (RAD != null)
{
var dH = VSL.Altitude.Relative - VSL.Altitude.Ahead - VSL.Geometry.H;
if (dH < 0)
{
dVv -= dH / RAD.TimeAhead * 1.1f;
}
}
if (dVv > 0)
{
fall_correction.I = BJ.FallCorrectionPID.I * Utils.ClampL(100 / VSL.Altitude.Relative, 1);
fall_correction.Update(-VSL.Altitude.Absolute);
}
else
{
fall_correction.IntegralError *= Utils.ClampL(1 - fall_correction.I / 10 * TimeWarp.fixedDeltaTime, 0);
fall_correction.Update(0);
}
// Log("dV {}, dVv {}, correction {}, I {}", dV, dVv, fall_correction.Action, fall_correction.I);//debug
if (Executor.Execute(dV + fall_correction * VSL.Physics.Up, 10))
{
Status("white",
"Accelerating: <color=yellow><b>{0}</b> m/s</color>",
(dV.magnitude - 10).ToString("F1"));
}
else
{
fine_tune_approach();
}
}
else
{
ATC.SetThrustDirW(-dV);
}
break;
case Stage.CorrectTrajectory:
VSL.Info.Countdown = landing_trajectory.BrakeStartPoint.UT - VSL.Physics.UT - ManeuverOffset;
if (VSL.Info.Countdown > 0)
{
warp_to_coundown();
if (!trajectory_computed())
{
break;
}
add_correction_node_if_needed();
stage = Stage.Coast;
}
else
{
stage = Stage.None;
start_landing();
}
break;
case Stage.Coast:
if (!coast_to_start())
{
stage = Stage.None;
}
break;
}
}