// double startAlpha; //debug
// double startUT = -1; //debug
// void log_flight() //debug
// {
// var target = ToOrbit != null? ToOrbit.Target : VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT+VesselOrbit.timeToAp);
// var alpha = Utils.ProjectionAngle(VesselOrbit.pos, target, target-VesselOrbit.pos) * Mathf.Deg2Rad;
// var arc = (startAlpha-alpha)*VesselOrbit.radius;
// CSV(VSL.Physics.UT-startUT, VSL.VerticalSpeed.Absolute, Vector3d.Exclude(VesselOrbit.pos, VesselOrbit.vel).magnitude,
// arc, VesselOrbit.radius-Body.Radius,
// VSL.Engines.Thrust.magnitude,
// VSL.Physics.M, 90-Vector3d.Angle(VesselOrbit.vel.xzy, VSL.Physics.Up));//debug
// }
protected override void Update()
{
if (!IsActive)
{
CFG.AP2.OffIfOn(Autopilot2.Rendezvous); return;
}
switch (stage)
{
case Stage.Start:
if (VSL.InOrbit &&
VesselOrbit.ApR > MinPeR &&
VesselOrbit.radius > MinPeR)
{
start_orbit();
}
else
{
to_orbit();
}
break;
case Stage.Launch:
if (ToOrbit.LaunchUT > VSL.Physics.UT)
{
Status("Waiting for launch window...");
correct_launch(false); //FIXME: sometimes AtmoSim returns inconsistent TTA which results in jumps of the Countdown.
VSL.Info.Countdown = ToOrbit.LaunchUT - VSL.Physics.UT;
VSL.Controls.WarpToTime = ToOrbit.LaunchUT;
break;
}
// if(startUT < 0) //debug
// {
// startAlpha = Utils.ProjectionAngle(VesselOrbit.pos, ToOrbit.Target, ToOrbit.Target-VesselOrbit.pos) * Mathf.Deg2Rad;
// startUT = VSL.Physics.UT;
// }
// log_flight();//debug
if (ToOrbit.Liftoff())
{
break;
}
stage = Stage.ToOrbit;
MinDist.Reset();
break;
case Stage.ToOrbit:
// log_flight();//debug
if (ToOrbit.GravityTurn(ManeuverOffset, GLB.ORB.GTurnCurve, GLB.ORB.Dist2VelF, REN.Dtol))
{
if (ToOrbit.dApA < REN.Dtol)
{
update_trajectory();
MinDist.Update(CurrentDistance);
if (MinDist)
{
start_orbit();
}
}
}
else
{
start_orbit();
}
break;
case Stage.StartOrbit:
// log_flight();//debug
Status("Achiving starting orbit...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
update_trajectory();
CurrentDistance = -1;
if (trajectory.RelDistanceToTarget < REN.CorrectionStart ||
trajectory.TimeToTarget / VesselOrbit.period > REN.MaxTTR)
{
stage = Stage.Rendezvou;
}
else
{
compute_rendezvou_trajectory();
}
break;
case Stage.ComputeRendezvou:
// log_flight();//debug
if (TimeWarp.CurrentRateIndex == 0 && TimeWarp.CurrentRate > 1)
{
Status("Waiting for Time Warp to end...");
break;
}
if (!trajectory_computed())
{
break;
}
#if DEBUG
if (trajectory.KillerOrbit ||
trajectory.Orbit.eccentricity > 0.6)
{
PauseMenu.Display();
}
#endif
if (!trajectory.KillerOrbit &&
trajectory.RelDistanceToTarget < REN.CorrectionStart &&
(CurrentDistance < 0 || trajectory.DistanceToTarget < CurrentDistance))
{
CorrectionTimer.Reset();
if (trajectory.ManeuverDeltaV.magnitude > 1)
{
VSL.Controls.StopWarp();
if (TimeWarp.CurrentRate > 1 ||
trajectory.TimeToStart < trajectory.ManeuverDuration / 2)
{
update_trajectory();
fine_tune_approach();
}
else
{
CurrentDistance = trajectory.DistanceToTarget;
add_trajectory_node();
CFG.AP1.On(Autopilot1.Maneuver);
stage = Stage.Rendezvou;
}
break;
}
update_trajectory();
if (CurrentDistance > REN.Dtol &&
trajectory.TimeToTarget > trajectory.BrakeDuration + ManeuverOffset + TimeWarp.CurrentRate)
{
stage = Stage.Coast;
break;
}
}
if (CurrentDistance > 0 &&
CurrentDistance / VesselOrbit.semiMajorAxis < REN.CorrectionStart)
{
VSL.Controls.StopWarp();
match_orbits();
break;
}
Status("red", "Failed to compute rendezvou trajectory.\nPlease, try again.");
CFG.AP2.Off();
#if DEBUG
PauseMenu.Display();
#endif
break;
case Stage.Rendezvou:
// log_flight();//debug
Status("Correcting trajectory...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
update_trajectory();
if (CurrentDistance < REN.ApproachThreshold)
{
match_orbits();
}
else
{
stage = Stage.Coast;
}
break;
case Stage.Coast:
Status("Coasting...");
if (!CorrectionTimer.TimePassed)
{
if (VSL.Controls.CanWarp)
{
VSL.Controls.WarpToTime = trajectory.AtTargetUT - trajectory.BrakeDuration + ManeuverOffset;
}
break;
}
CorrectionTimer.Reset();
update_trajectory();
fine_tune_approach();
break;
case Stage.MatchOrbits:
// log_flight();//debug
Status("Matching orbits at nearest approach...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
update_trajectory();
update_direct_distance();
if (TargetLoaded)
{
approach_or_brake();
}
else
{
var relD = DirectDistance / VesselOrbit.semiMajorAxis;
if (relD > REN.CorrectionStart)
{
start_orbit();
}
else if (relD > REN.CorrectionStart / 4)
{
fine_tune_approach();
}
else
{
approach_or_brake();
}
}
break;
case Stage.Approach:
Status("Approaching...");
THR.DeltaV = 0;
var dP = TargetOrbit.pos - VesselOrbit.pos;
var dPm = dP.magnitude;
if (dPm - VSL.Geometry.MinDistance < REN.Dtol)
{
brake(); break;
}
var throttle = VSL.Controls.OffsetAlignmentFactor();
if (throttle.Equals(0))
{
break;
}
var dV = Vector3d.Dot(VesselOrbit.vel - TargetOrbit.vel, dP / dPm);
var nV = Utils.Clamp(dPm * REN.ApproachVelF, 1, REN.MaxApproachV);
if (dV + GLB.THR.MinDeltaV < nV)
{
VSL.Engines.ActivateEngines();
THR.DeltaV = (float)(nV - dV) * throttle * throttle;
}
else
{
brake();
}
break;
case Stage.Brake:
Status("Braking near target...");
if (CFG.AP1[Autopilot1.MatchVelNear])
{
break;
}
if (CFG.AP1[Autopilot1.MatchVel])
{
if ((TargetOrbit.vel - VesselOrbit.vel).magnitude > GLB.THR.MinDeltaV)
{
break;
}
CFG.AP1.Off();
THR.Throttle = 0;
}
if ((VSL.Physics.wCoM - TargetVessel.CurrentCoM).magnitude - VSL.Geometry.R - TargetVessel.Radius() > REN.Dtol)
{
approach(); break;
}
CFG.AP2.Off();
CFG.AT.OnIfNot(Attitude.KillRotation);
ClearStatus();
break;
}
}
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;
}
}
// double startAlpha; //debug
// double startUT = -1; //debug
// void log_flight() //debug
// {
// var target = ToOrbit != null? ToOrbit.Target : VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT+VesselOrbit.timeToAp);
// var alpha = Utils.ProjectionAngle(VesselOrbit.pos, target, target-VesselOrbit.pos) * Mathf.Deg2Rad;
// var arc = (startAlpha-alpha)*VesselOrbit.radius;
// CSV(VSL.Physics.UT-startUT, VSL.VerticalSpeed.Absolute, Vector3d.Exclude(VesselOrbit.pos, VesselOrbit.vel).magnitude,
// arc, VesselOrbit.radius-Body.Radius,
// VSL.Engines.Thrust.magnitude,
// VSL.Physics.M, 90-Vector3d.Angle(VesselOrbit.vel.xzy, VSL.Physics.Up));//debug
// }
protected override void Update()
{
if (!IsActive)
{
CFG.AP2.OffIfOn(Autopilot2.Rendezvous); return;
}
switch (stage)
{
case Stage.Start:
if (VSL.InOrbit &&
VesselOrbit.ApR > MinPeR &&
VesselOrbit.radius > MinPeR)
{
update_trajectory();
if (CurrentDistance > REN.CorrectionStart)
{
start_orbit();
}
else
{
fine_tune_approach();
}
}
else
{
to_orbit();
}
break;
case Stage.Launch:
if (ToOrbit.LaunchUT > VSL.Physics.UT)
{
Status("Waiting for launch window...");
correct_launch(false); //FIXME: sometimes AtmoSim returns inconsistent TTA which results in jumps of the Countdown.
VSL.Info.Countdown = ToOrbit.LaunchUT - VSL.Physics.UT;
VSL.Controls.WarpToTime = ToOrbit.LaunchUT;
break;
}
// if(startUT < 0) //debug
// {
// startAlpha = Utils.ProjectionAngle(VesselOrbit.pos, ToOrbit.Target, ToOrbit.Target-VesselOrbit.pos) * Mathf.Deg2Rad;
// startUT = VSL.Physics.UT;
// }
// log_flight();//debug
if (ToOrbit.Liftoff())
{
break;
}
stage = Stage.ToOrbit;
MinDist.Reset();
break;
case Stage.ToOrbit:
// log_flight();//debug
if (ToOrbit.GravityTurn(TRJ.ManeuverOffset, GLB.ORB.GTurnCurve, GLB.ORB.Dist2VelF, REN.Dtol))
{
if (ToOrbit.dApA < REN.Dtol)
{
update_trajectory();
MinDist.Update(CurrentDistance);
if (MinDist)
{
start_orbit();
}
}
}
else
{
start_orbit();
}
break;
case Stage.StartOrbit:
// log_flight();//debug
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
update_trajectory();
CurrentDistance = -1;
if (trajectory.DistanceToTarget < REN.CorrectionStart ||
(trajectory.TransferTime + trajectory.TimeToStart) / VesselOrbit.period > REN.MaxTTR)
{
stage = Stage.Rendezvou;
}
else
{
compute_rendezvou_trajectory();
}
break;
case Stage.ComputeRendezvou:
// log_flight();//debug
if (!trajectory_computed())
{
break;
}
if (trajectory.ManeuverDeltaV.magnitude > GLB.THR.MinDeltaV * 5 &&
trajectory.DistanceToTarget < REN.CorrectionStart &&
(CurrentDistance < 0 || trajectory.DistanceToTarget < CurrentDistance))
{
CorrectionTimer.Start();
CurrentDistance = trajectory.DistanceToTarget;
add_trajectory_node();
CFG.AP1.On(Autopilot1.Maneuver);
stage = Stage.Rendezvou;
}
else if (trajectory.DistanceToTarget < REN.CorrectionStart)
{
match_orbits();
}
else
{
Status("red", "Failed to compute rendezvou trajectory.\nPlease, try again.");
CFG.AP2.Off();
return;
}
break;
case Stage.Rendezvou:
// log_flight();//debug
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
if (!CorrectionTimer.TimePassed && CurrentDistance >= 0)
{
break;
}
CorrectionTimer.Reset();
update_trajectory();
if (CurrentDistance < REN.ApproachThreshold)
{
match_orbits();
}
else
{
fine_tune_approach();
}
break;
case Stage.MatchOrbits:
// log_flight();//debug
Status("Matching orbits at nearest approach...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
update_trajectory();
var dist = Utils.ClampL((TargetOrbit.pos - VesselOrbit.pos).magnitude - VSL.Geometry.R - TargetVessel.Radius(), 0);
if (dist > REN.CorrectionStart)
{
start_orbit();
}
else if (dist > REN.CorrectionStart / 4)
{
fine_tune_approach();
}
else if (dist > REN.Dtol)
{
approach();
}
else
{
brake();
}
break;
case Stage.Approach:
Status("Approaching...");
var dP = TargetOrbit.pos - VesselOrbit.pos;
var dPm = dP.magnitude;
if (dPm - VSL.Geometry.R - TargetVessel.Radius() < REN.Dtol)
{
brake(); break;
}
if (VSL.Controls.AttitudeError > 1)
{
break;
}
var dV = Vector3d.Dot(VesselOrbit.vel - TargetOrbit.vel, dP / dPm);
var nV = Utils.Clamp(dPm * REN.ApproachVelF, 1, REN.MaxApproachV);
if (dV + GLB.THR.MinDeltaV < nV)
{
VSL.Engines.ActivateEngines();
THR.DeltaV = (float)(nV - dV);
}
else
{
brake();
}
break;
case Stage.Brake:
Status("Braking near target...");
if (CFG.AP1[Autopilot1.MatchVelNear])
{
break;
}
if (CFG.AP1[Autopilot1.MatchVel])
{
if ((TargetOrbit.vel - VesselOrbit.vel).magnitude > GLB.THR.MinDeltaV)
{
break;
}
CFG.AP1.Off();
THR.Throttle = 0;
}
if ((VSL.Physics.wCoM - TargetVessel.CurrentCoM).magnitude - VSL.Geometry.R - TargetVessel.Radius() > REN.Dtol)
{
approach(); break;
}
CFG.AP2.Off();
CFG.AT.OnIfNot(Attitude.KillRotation);
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;
}
}
