public override void Draw()
{
#if DEBUG
if (ToOrbit != null)
{
Utils.GLVec(Body.position, ToOrbit.Target.xzy, Color.green);
Utils.GLVec(Body.position, VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT + VesselOrbit.timeToAp).xzy, Color.magenta);
Utils.GLVec(Body.position, VesselOrbit.GetOrbitNormal().normalized.xzy *Body.Radius * 1.1, Color.cyan);
Utils.GLVec(Body.position, Vector3d.Cross(VesselOrbit.pos, ToOrbit.Target).normalized.xzy *Body.Radius * 1.1, Color.red);
}
#endif
if (stage == Stage.None)
{
if (Utils.ButtonSwitch("ToOrbit", ShowOptions,
"Achieve a circular orbit with desired radius and inclination",
GUILayout.ExpandWidth(true)))
{
toggle_orbit_editor();
}
}
else if (GUILayout.Button(new GUIContent("ToOrbit", "Change target orbit or abort"),
Styles.danger_button, GUILayout.ExpandWidth(true)))
{
toggle_orbit_editor();
}
}
public bool GravityTurn(double ApA_offset, double gturn_curve, double dist2vel, double Dtol)
{
UpdateTargetPosition();
VSL.Engines.ActivateNextStageOnFlameout();
dApA = TargetR-VesselOrbit.ApR;
var vel = Vector3d.zero;
var cApV = VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT+VesselOrbit.timeToAp);
var hv = Vector3d.Exclude(VesselOrbit.pos, target-VesselOrbit.pos).normalized;
var arc = Utils.ProjectionAngle(cApV, target, hv)*Mathf.Deg2Rad*cApV.magnitude;
ErrorThreshold.Value = CorrectOnlyAltitude? dApA : dApA+arc;
ApoapsisReached |= dApA < Dtol;
THR.CorrectThrottle = ApoapsisReached;
if(!ErrorThreshold)
{
var startF = Utils.Clamp((VSL.Altitude.Absolute-GravityTurnStart)/GLB.ORB.GTurnOffset, 0, 1);
if(dApA > Dtol)
vel += CorrectOnlyAltitude && ApoapsisReached?
VesselOrbit.vel.normalized.xzy*dApA :
VSL.Physics.Up.xzy*dApA*gturn_curve;
if(arc > Dtol && (!ApoapsisReached || !CorrectOnlyAltitude))
{
var hvel = Utils.ClampL(arc-dApA*gturn_curve, 0)*startF;
if(Body.atmosphere) hvel *= Math.Sqrt(Utils.Clamp(VSL.Altitude.Absolute/Body.atmosphereDepth, 0, 1));
vel += hv*hvel;
}
vel *= VSL.Physics.StG/Utils.G0*dist2vel;
if(!vel.IsZero())
{
var norm = VesselOrbit.GetOrbitNormal();
var dFi = (90-Vector3d.Angle(norm, target))*Mathf.Deg2Rad;
vel += norm*Math.Sin(dFi)*vel.magnitude*startF
*Utils.Clamp(VSL.VerticalSpeed.Absolute/VSL.Physics.G-MinClimbTime, 0, 100)
*Utils.ClampL(Vector3d.Dot(hv, VesselOrbit.vel.normalized), 0);
}
vel = vel.xzy;
CircularizationOffset = -1;
if(Executor.Execute(vel, Utils.Clamp(1-VSL.Torque.MaxCurrent.AA_rad, 0.1f, 1)))
{
if(CFG.AT.Not(Attitude.KillRotation))
{
CFG.BR.OnIfNot(BearingMode.Auto);
BRC.ForwardDirection = hv.xzy;
}
Status("Gravity turn...");
return true;
}
}
Status("Coasting...");
CFG.BR.OffIfOn(BearingMode.Auto);
CFG.AT.OnIfNot(Attitude.KillRotation);
THR.Throttle = 0;
if(CircularizationOffset < 0)
{
ApAUT = VSL.Physics.UT+VesselOrbit.timeToAp;
CircularizationOffset = VSL.Engines.TTB((float)TrajectoryCalculator.dV4C(VesselOrbit, hV(ApAUT), ApAUT).magnitude)/2;
}
return VesselOrbit.timeToAp > ApA_offset+CircularizationOffset &&
Body.atmosphere && VesselOrbit.radius < Body.Radius+Body.atmosphereDepth;
}
protected Vector3d Node2OrbitDeltaV(Vector3d NodeDeltaV, double StartUT)
{
var norm = VesselOrbit.GetOrbitNormal().normalized;
var prograde = hV(StartUT).normalized;
var radial = Vector3d.Cross(prograde, norm).normalized;
return(radial * NodeDeltaV.x + norm * NodeDeltaV.y + prograde * NodeDeltaV.z);
}
//Node: radial, normal, prograde
protected Vector3d Orbit2NodeDeltaV(Vector3d OrbitDeltaV, double StartUT)
{
var norm = VesselOrbit.GetOrbitNormal().normalized;
var prograde = hV(StartUT).normalized;
var radial = Vector3d.Cross(prograde, norm).normalized;
return(new Vector3d(Vector3d.Dot(OrbitDeltaV, radial),
Vector3d.Dot(OrbitDeltaV, norm),
Vector3d.Dot(OrbitDeltaV, prograde)));
}
protected Vector3d tune_needed_vel(Vector3d needed_vel, Vector3d pg_vel, double startF)
{
if (CourseOnTarget)
{
norm_correction.Update((float)(90 - Utils.Angle2(VesselOrbit.GetOrbitNormal(), target)));
needed_vel = QuaternionD.AngleAxis(norm_correction.Action, VesselOrbit.pos) * needed_vel;
}
else
{
norm_correction.Update(0);
}
return(startF < 1 ?
needed_vel.magnitude * Vector3d.Lerp(pg_vel.normalized,
needed_vel.normalized,
startF)
: needed_vel);
}
IEnumerator<YieldInstruction> compute_initial_eccentricity()
{
// Log("Calculating initial orbit eccentricity...");//debug
var tPos = CFG.Target.RelOrbPos(Body);
var UT = VSL.Physics.UT +
AngleDelta(VesselOrbit, tPos, VSL.Physics.UT)/360*VesselOrbit.period;
var vPos = VesselOrbit.getRelativePositionAtUT(UT);
var vVel = VesselOrbit.getOrbitalVelocityAtUT(UT);
var dir = Vector3d.Exclude(vPos, vVel);
var ini_dV = dV4Pe(VesselOrbit, (Body.Radius+TargetAltitude)*0.999, UT);
var trj = new LandingTrajectory(VSL, ini_dV, UT, CFG.Target, TargetAltitude);
var atmo_curve = trj.GetAtmosphericCurve(5);
var maxV = vVel.magnitude;
var minV = 0.0;
var dV = 0.0;
dir = -dir.normalized;
var in_plane = Math.Abs(90-Vector3.Angle(tPos, VesselOrbit.GetOrbitNormal())) < 5;
// Log("in plane {}, ini dV {}\nini trj:\n{}", in_plane, ini_dV, trj);//debug
yield return null;
while(maxV-minV > 1)
{
dV = (maxV+minV)/2;
trj = new LandingTrajectory(VSL, ini_dV+dir*dV, UT, CFG.Target, trj.TargetAltitude);
atmo_curve = trj.GetAtmosphericCurve(5);
// Log("dV: {} : {} : {} m/s\ntrj:\n{}", minV, dV, maxV, trj);//debug
if(!trj.NotEnoughFuel && (in_plane || trj.DeltaR < 0) &&
(atmo_curve != null &&
(will_overheat(atmo_curve) ||
atmo_curve[atmo_curve.Count-1].DynamicPressure > DEO.MaxDynPressure) ||
!Body.atmosphere && trj.LandingAngle < DEO.MinLandingAngle))
minV = dV;
else maxV = dV;
yield return null;
}
currentEcc = trj.Orbit.eccentricity;
dEcc = currentEcc/DEO.EccSteps;
// Log("currentEcc: {}, dEcc {}", currentEcc, dEcc);//debug
}
protected override void Update()
{
switch (stage)
{
case Stage.Start:
if (VSL.LandedOrSplashed || VSL.VerticalSpeed.Absolute < 5)
{
stage = Stage.Liftoff;
}
else
{
ToOrbit.StartGravityTurn();
stage = Stage.GravityTurn;
}
break;
case Stage.Liftoff:
if (ToOrbit.Liftoff())
{
break;
}
stage = Stage.GravityTurn;
break;
case Stage.GravityTurn:
update_inclination_limits();
var norm = VesselOrbit.GetOrbitNormal();
var needed_norm = Vector3d.Cross(VesselOrbit.pos, ToOrbit.Target.normalized);
var norm2norm = Math.Abs(Utils.Angle2(norm, needed_norm) - 90);
if (norm2norm > 60)
{
var ApV = VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT + VesselOrbit.timeToAp);
var arcApA = AngleDelta(VesselOrbit, ApV);
var arcT = AngleDelta(VesselOrbit, ToOrbit.Target);
if (arcT > 0 && arcT < arcApA)
{
ApV.Normalize();
var chord = ApV * VesselOrbit.radius - VesselOrbit.pos;
var alpha = inclination_correction(VesselOrbit.inclination, chord.magnitude);
var axis = Vector3d.Cross(norm, ApV);
ToOrbit.Target = QuaternionD.AngleAxis(alpha, axis) * ApV * ToOrbit.TargetR;
}
else
{
var inclination = Math.Acos(needed_norm.z / needed_norm.magnitude) * Mathf.Rad2Deg;
var chord = Vector3d.Exclude(norm, ToOrbit.Target).normalized *VesselOrbit.radius - VesselOrbit.pos;
var alpha = inclination_correction(inclination, chord.magnitude);
var axis = Vector3d.Cross(norm, VesselOrbit.pos.normalized);
if (arcT < 0)
{
alpha = -alpha;
}
ToOrbit.Target = QuaternionD.AngleAxis(alpha, axis) * ToOrbit.Target;
}
}
if (ToOrbit.GravityTurn(C.Dtol))
{
break;
}
CFG.BR.OffIfOn(BearingMode.Auto);
var ApAUT = VSL.Physics.UT + VesselOrbit.timeToAp;
if (ApR > ToOrbit.MaxApR)
{
change_ApR(ApAUT);
}
else
{
circularize(ApAUT);
}
break;
case Stage.ChangeApA:
TmpStatus("Achieving target apoapsis...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
circularize(VSL.Physics.UT + VesselOrbit.timeToAp);
stage = Stage.Circularize;
break;
case Stage.Circularize:
TmpStatus("Circularization...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
Disable();
ClearStatus();
break;
}
}
protected override void Update()
{
switch (stage)
{
case Stage.Start:
if (VSL.LandedOrSplashed)
{
stage = Stage.Liftoff;
}
else
{
ToOrbit.StartGravityTurn();
stage = Stage.GravityTurn;
}
break;
case Stage.Liftoff:
if (ToOrbit.Liftoff())
{
break;
}
stage = Stage.GravityTurn;
break;
case Stage.GravityTurn:
update_inclination_limits();
var norm = VesselOrbit.GetOrbitNormal();
var ApV = VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT + VesselOrbit.timeToAp);
var arcT = AngleDelta(VesselOrbit, ToOrbit.Target);
if (arcT > 0 && arcT < AngleDelta(VesselOrbit, ApV))
{
ApV.Normalize();
var chord = ApV * VesselOrbit.radius - VesselOrbit.pos;
var alpha = inclination_correction(VesselOrbit.inclination, chord.magnitude);
ToOrbit.Target = QuaternionD.AngleAxis(alpha, Vector3d.Cross(norm, ApV))
* ApV * ToOrbit.TargetR;
}
else
{
var n = Vector3d.Cross(VesselOrbit.pos, ToOrbit.Target.normalized);
var inclination = Math.Acos(n.z / n.magnitude) * Mathf.Rad2Deg;
var chord = Vector3d.Exclude(norm, ToOrbit.Target).normalized *VesselOrbit.radius - VesselOrbit.pos;
var alpha = inclination_correction(inclination, chord.magnitude);
ToOrbit.Target = QuaternionD.AngleAxis(alpha, Vector3d.Cross(norm, VesselOrbit.pos))
* ToOrbit.Target;
}
if (ToOrbit.GravityTurn(ManeuverOffset, ORB.GTurnCurve, ORB.Dist2VelF, ORB.Dtol))
{
break;
}
CFG.BR.OffIfOn(BearingMode.Auto);
var ApAUT = VSL.Physics.UT + VesselOrbit.timeToAp;
if (ApR > MinPeR + ORB.RadiusOffset)
{
change_ApR(ApAUT);
}
else
{
circularize(ApAUT);
}
break;
case Stage.ChangeApA:
TmpStatus("Achieving target apoapsis...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
circularize(VSL.Physics.UT + VesselOrbit.timeToAp);
stage = Stage.Circularize;
break;
case Stage.Circularize:
TmpStatus("Circularization...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
Disable();
ClearStatus();
break;
}
}
protected override void Update()
{
switch (stage)
{
case Stage.Start:
if (VSL.LandedOrSplashed || VSL.VerticalSpeed.Absolute < 5)
{
stage = Stage.Liftoff;
}
else
{
ToOrbit.StartGravityTurn();
inclinationPID.Reset();
stage = Stage.GravityTurn;
}
break;
case Stage.Liftoff:
if (ToOrbit.Liftoff())
{
break;
}
inclinationPID.Reset();
stage = Stage.GravityTurn;
break;
case Stage.GravityTurn:
update_inclination_limits();
var orbitNormal = VesselOrbit.GetOrbitNormal();
var targetNormalized = ToOrbit.Target.normalized;
var vsl2TargetNormal = Vector3d.Cross(VesselOrbit.pos, targetNormalized);
var norm2norm = Math.Abs(Utils.Angle2(orbitNormal, vsl2TargetNormal) - 90);
if (norm2norm > 60)
{
var ApV = VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT + VesselOrbit.timeToAp);
var arcApA = Utils.Angle2(VesselOrbit.pos, ApV);
var arcApAT = Utils.Angle2(ApV, ToOrbit.Target);
if (arcApAT < arcApA)
{
ToOrbit.Target = QuaternionD.AngleAxis(arcApA - arcApAT, vsl2TargetNormal) * ToOrbit.Target;
}
var inclination = Math.Acos(vsl2TargetNormal.z / vsl2TargetNormal.magnitude) * Mathf.Rad2Deg;
inclinationPID.Update((float)inclination_error(inclination));
var axis = Vector3d.Cross(vsl2TargetNormal, targetNormalized);
ToOrbit.Target = QuaternionD.AngleAxis(inclinationPID.Action, axis) * ToOrbit.Target;
}
if (ToOrbit.GravityTurn(C.Dtol))
{
break;
}
CFG.BR.OffIfOn(BearingMode.Auto);
var ApAUT = VSL.Physics.UT + VesselOrbit.timeToAp;
if (ApR > ToOrbit.MaxApR)
{
change_ApR(ApAUT);
}
else
{
circularize(ApAUT);
}
break;
case Stage.ChangeApA:
TmpStatus("Achieving target apoapsis...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
circularize(VSL.Physics.UT + VesselOrbit.timeToAp);
stage = Stage.Circularize;
break;
case Stage.Circularize:
TmpStatus("Circularization...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
Disable();
ClearStatus();
break;
}
}
