public List <AtmosphericConditions> GetAtmosphericCurve(double dT, double endUT = -1)
{
if (!Body.atmosphere)
{
return(null);
}
var atmoR = Body.Radius + Body.atmosphereDepth;
var startUT = StartPos.magnitude < atmoR? StartUT : TrajectoryCalculator.NearestRadiusUT(Orbit, atmoR, StartUT);
if (endUT < 0)
{
endUT = BrakeEndUT;
}
if (startUT > endUT)
{
return(null);
}
var samples = (int)Math.Ceiling((endUT - startUT) / dT) + 1;
var curve = new List <AtmosphericConditions>(samples);
dT = (endUT - startUT) / samples;
for (int i = 1; i <= samples; i++)
{
var cond = new AtmosphericConditions(Orbit, startUT + dT * i);
cond.Duration = dT;
curve.Add(cond);
}
return(curve);
}
void update()
{
if (TransferTime < 0)
{
TrajectoryCalculator.ClosestApproach(Orbit, TargetOrbit, StartUT, VSL.Geometry.MinDistance, out AtTargetUT);
TransferTime = AtTargetUT - StartUT;
}
else
{
AtTargetUT = StartUT + TransferTime;
}
AtTargetPos = Orbit.getRelativePositionAtUT(AtTargetUT);
AtTargetVel = Orbit.getOrbitalVelocityAtUT(AtTargetUT);
TargetPos = TargetOrbit.getRelativePositionAtUT(AtTargetUT);
DistanceToTarget = Utils.ClampL((AtTargetPos - TargetPos).magnitude - VSL.Geometry.MinDistance, 0);
DeltaTA = Utils.ProjectionAngle(AtTargetPos, TargetPos,
Vector3d.Cross(Orbit.GetOrbitNormal(), AtTargetPos)) *
Math.Sign(TargetOrbit.period - OrigOrbit.period);
DeltaFi = TrajectoryCalculator.RelativeInclination(Orbit, TargetPos);
DeltaR = Vector3d.Dot(TargetPos - AtTargetPos, AtTargetPos.normalized);
var t_orbit = Target.GetOrbit();
var t_vel = t_orbit != null?t_orbit.getOrbitalVelocityAtUT(AtTargetUT) : Vector3d.zero;
BrakeDeltaV = t_vel - Orbit.getOrbitalVelocityAtUT(AtTargetUT);
BrakeDuration = VSL.Engines.TTB((float)BrakeDeltaV.magnitude);
KillerOrbit = Orbit.PeR < MinPeR && Orbit.timeToPe < TransferTime;
// Utils.Log("{}", this);//debug
}
void update()
{
if (TransferTime < 0)
{
TrajectoryCalculator.ClosestApproach(Orbit, TargetOrbit, StartUT, VSL.Geometry.MinDistance, out AtTargetUT);
TransferTime = AtTargetUT - StartUT;
}
else
{
AtTargetUT = StartUT + TransferTime;
}
var obt = TrajectoryCalculator.NextOrbit(Orbit, AtTargetUT);
var t_orbit = TrajectoryCalculator.NextOrbit(TargetOrbit, AtTargetUT);
AtTargetPos = obt.getRelativePositionAtUT(AtTargetUT);
AtTargetVel = obt.getOrbitalVelocityAtUT(AtTargetUT);
TargetPos = TrajectoryCalculator.RelativePosAtUT(obt.referenceBody, t_orbit, AtTargetUT);
AtTargetRelPos = AtTargetPos - TargetPos;
DistanceToTarget = AtTargetRelPos.magnitude - VSL.Geometry.MinDistance;
DirectHit = DistanceToTarget < 1;
DistanceToTarget = Utils.ClampL(DistanceToTarget, 0);
BrakeDeltaV = t_orbit.GetFrameVelAtUT(AtTargetUT) - obt.GetFrameVelAtUT(AtTargetUT);
var brake_dV = (float)BrakeDeltaV.magnitude;
BrakeDuration = VSL.Engines.TTB_Precise(brake_dV);
BrakeFuel = VSL.Engines.FuelNeeded(brake_dV);
FullBrake = GetTotalFuel() < VSL.Engines.AvailableFuelMass;
//check if this trajectory is too close to any of celestial bodies it passes by
KillerOrbit = TransferTime < BrakeDuration + ManeuverDuration;
update_killer(OrigOrbit, StartUT);
update_killer(Orbit, AtTargetUT);
// Utils.Log("{}", this);//debug
}
void update_from_orbit(Orbit orb, double UT)
{
//calculate the position of a landing site
if (orb.ApA <= TargetAltitude)
{
AtTargetUT = orb.StartUT + (orb.ApAhead()? orb.timeToAp : 1);
}
else if (orb.PeA < TargetAltitude)
{
AtTargetUT = TrajectoryCalculator.NearestRadiusUT(orb, Body.Radius + TargetAltitude, UT);
}
else
{
AtTargetUT = orb.StartUT + orb.timeToPe;
}
TransferTime = AtTargetUT - StartUT;
AtTargetPos = orb.getRelativePositionAtUT(AtTargetUT);
AtTargetVel = orb.getOrbitalVelocityAtUT(AtTargetUT);
var at_target_rot = TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToTarget);
approach = new Coordinates((at_target_rot * (AtTargetPos - AtTargetVel * 10)).xzy + Body.position, Body);
SurfacePoint = new WayPoint((at_target_rot * AtTargetPos).xzy + Body.position, Body);
SurfacePoint.Pos.SetAlt2Surface(Body);
SurfacePoint.Name = "Landing Site";
}
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;
}
void update_landing_site(LandingPath path)
{
var at_target_rot = TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToTarget);
approach = new Coordinates((at_target_rot * path.PointAtUT(path.EndUT - 1).pos).xzy + Body.position, Body);
SurfacePoint = new WayPoint((at_target_rot * AtTargetPos).xzy + Body.position, Body);
SurfacePoint.Pos.SetAlt2Surface(Body);
SurfacePoint.Name = "Landing Site";
}
protected override void Update()
{
if (!IsActive)
{
return;
}
Vector3 dV;
if (CFG.AP1[Autopilot1.MatchVel])
{
dV = CFG.Target.GetObtVelocity() - VSL.vessel.obt_velocity;
VSL.Engines.RequestClusterActivationForManeuver(dV);
Executor.Execute(dV, GLB.THR.MinDeltaV);
}
else
{
double ApprUT;
var tOrb = CFG.Target.GetOrbit();
var dist = TrajectoryCalculator.NearestApproach(VSL.orbit, tOrb, VSL.Physics.UT, VSL.Geometry.MinDistance + 10, out ApprUT);
TTA = (float)(ApprUT - VSL.Physics.UT);
switch (stage)
{
case Stage.Start:
if (dist > MVA.MaxApproachDistance)
{
Status(string.Format("<color=yellow>WARNING:</color> Nearest approach distance is <color=magenta><b>{0}</b></color>\n" +
"<color=red><b>Push to proceed. At your own risk.</b></color>",
Utils.formatBigValue((float)dist, "m")));
stage = Stage.Wait;
goto case Stage.Wait;
}
stage = Stage.Brake;
goto case Stage.Brake;
case Stage.Wait:
if (!string.IsNullOrEmpty(TCAGui.StatusMessage))
{
break;
}
stage = Stage.Brake;
goto case Stage.Brake;
case Stage.Brake:
dV = (tOrb.getOrbitalVelocityAtUT(ApprUT) - VSL.orbit.getOrbitalVelocityAtUT(ApprUT)).xzy;
VSL.Engines.RequestClusterActivationForManeuver(dV);
if (Executor.Execute(dV, GLB.THR.MinDeltaV, StartCondition))
{
break;
}
reset();
break;
}
}
}
public IEnumerator <LandingTrajectory> GetEnumerator()
{
// m.Log("Calculating initial orbit eccentricity...");//debug
var tPos = m.CFG.Target.OrbPos(m.Body);
var UT = m.VSL.Physics.UT +
TrajectoryCalculator.AngleDelta(m.VesselOrbit, tPos, m.VSL.Physics.UT) / 360 * m.VesselOrbit.period;
if (UT < m.VSL.Physics.UT)
{
UT += m.VesselOrbit.period;
}
var vPos = m.VesselOrbit.getRelativePositionAtUT(UT);
var vVel = m.VesselOrbit.getOrbitalVelocityAtUT(UT);
var incl = Math.Abs(90 - Utils.Angle2(tPos, m.VesselOrbit.GetOrbitNormal()));
var ini_dV = TrajectoryCalculator.dV4Pe(m.VesselOrbit, (m.Body.Radius + m.TargetAltitude * 0.9), UT);
ini_dV = QuaternionD.AngleAxis(incl, vPos) * (ini_dV + vVel);
var dir = -ini_dV.normalized;
var maxV = ini_dV.magnitude;
ini_dV -= vVel;
Best = new LandingTrajectory(m.VSL, ini_dV, UT, m.CFG.Target, m.TargetAltitude);
yield return(Best);
//if there's not enough fuel, just go with the smallest maneuver possible
if (Best.FullManeuver)
{
//search for the best trajectory using current comparer
var start = Best;
var bestV = 0.0;
var dV = maxV / 10;
var V = dV;
while (Math.Abs(dV) > TrajectoryCalculator.C.dVtol)
{
var cur = new LandingTrajectory(m.VSL, start.ManeuverDeltaV + dir * V, start.StartUT, m.CFG.Target, start.TargetAltitude);
// m.Log("V {}, dV {}, is better {}\ncur {}\nbest {}",
// V, dV, comparer.isBetter(cur, Best), cur, Best);//debug
if (comparer.isBetter(cur, Best))
{
Best = cur;
bestV = V;
}
V += dV;
if (V < 0 || V > maxV || cur != Best)
{
dV /= -2.1;
V = bestV;
}
yield return(cur);
}
}
m.initialEcc = Best.Orbit.eccentricity;
// m.Log("initialEcc: {}", m.initialEcc);//debug
}
protected override void Update()
{
Vector3 dV;
if (CFG.AP1[Autopilot1.MatchVel])
{
Working = true;
dV = CFG.Target.GetObtVelocity() - VSL.vessel.obt_velocity;
if (!Executor.Execute(dV, MinDeltaV))
{
Executor.Reset();
}
}
else
{
var tOrb = CFG.Target.GetOrbit();
var dist = Working? 0 :
TrajectoryCalculator.NearestApproach(VSL.orbit, tOrb, VSL.Physics.UT, VSL.Geometry.MinDistance + 10, out ApprUT);
TTA = (float)(ApprUT - VSL.Physics.UT);
switch (stage)
{
case Stage.Start:
if (dist > C.MaxApproachDistance)
{
Status(Colors.Warning.Tag("WARNING: ") +
"Nearest approach distance is " +
Colors.Selected2.Tag("<b>{0}</b>\n") +
Colors.Danger.Tag("<b>Push to proceed. At your own risk.</b>"),
Utils.formatBigValue((float)dist, "m"));
stage = Stage.Wait;
goto case Stage.Wait;
}
stage = Stage.Brake;
goto case Stage.Brake;
case Stage.Wait:
if (!string.IsNullOrEmpty(TCAGui.StatusMessage))
{
break;
}
stage = Stage.Brake;
goto case Stage.Brake;
case Stage.Brake:
dV = (TrajectoryCalculator.NextOrbit(tOrb, ApprUT).GetFrameVelAtUT(ApprUT) - VSL.orbit.GetFrameVelAtUT(ApprUT)).xzy;
if (!Executor.Execute(dV, MinDeltaV, StartCondition))
{
Reset();
}
break;
}
}
}
protected BaseTrajectory(VesselWrapper vsl, Vector3d dV, double startUT)
{
VSL = vsl;
ManeuverDeltaV = dV;
ManeuverDuration = VSL.Engines.TTB((float)ManeuverDeltaV.magnitude);
StartUT = startUT;
TimeToStart = startUT - VSL.Physics.UT;
Body = VSL.vessel.orbitDriver.orbit.referenceBody;
OrigOrbit = VSL.vessel.orbitDriver.orbit;
Orbit = TrajectoryCalculator.NewOrbit(OrigOrbit, ManeuverDeltaV, StartUT);
StartPos = Orbit.getRelativePositionAtUT(StartUT);
StartVel = Orbit.getOrbitalVelocityAtUT(StartUT);
}
protected bool coast(Vector3d pg_vel)
{
Status("Coasting...");
CFG.BR.OffIfOn(BearingMode.Auto);
CFG.AT.OnIfNot(Attitude.Custom);
ATC.SetThrustDirW(-pg_vel.xzy);
THR.Throttle = 0;
if (CircularizationOffset < 0)
{
ApAUT = VSL.Physics.UT + VesselOrbit.timeToAp;
CircularizationOffset = VSL.Engines.TTB_Precise((float)TrajectoryCalculator.dV4C(VesselOrbit, hV(ApAUT), ApAUT).magnitude) / 2;
}
return(VesselOrbit.timeToAp > TimeToApA + CircularizationOffset &&
Body.atmosphere && VesselOrbit.radius < Body.Radius + Body.atmosphereDepth);
}
void update_from_orbit(Orbit orb, double UT)
{
//calculate the position of a landing site
if (orb.ApA <= TargetAltitude)
{
AtTargetUT = orb.StartUT + (orb.ApAhead()? orb.timeToAp : 1);
}
else
{
AtTargetUT = TrajectoryCalculator.NearestRadiusUT(orb, Body.Radius + TargetAltitude, UT);
}
TransferTime = AtTargetUT - StartUT;
AtTargetPos = orb.getRelativePositionAtUT(AtTargetUT);
AtTargetVel = orb.getOrbitalVelocityAtUT(AtTargetUT);
SurfacePoint = new WayPoint((TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToSurface) * AtTargetPos).xzy + Body.position, Body);
SurfacePoint.Name = "Landing Site";
}
public double Altitude2UT(double altitude)
{
if (!HavePoints || altitude > Points[0].Altitude)
{
return(TrajectoryCalculator.NearestRadiusUT(Orbit, altitude + Orbit.referenceBody.Radius, StartUT));
}
for (int i = 1, count = Points.Count; i < count; i++)
{
var p1 = Points[i];
if (p1.Altitude > altitude)
{
continue;
}
var p0 = Points[i - 1];
return(p0.UT + p0.Duration * (altitude - p0.Altitude) / (p1.Altitude - p0.Altitude));
}
return(EndUT);
}
void update_from_orbit()
{
AtTargetUT = Body.atmosphere && Orbit.altitude > Body.atmosphereDepth ?
TrajectoryCalculator.NearestRadiusUT(Orbit, Body.Radius + Body.atmosphereDepth, StartUT) + 1 : StartUT;
var end_alt = Math.Max(Orbit.PeA + 10, TargetAltitude);
Path = new LandingPath(VSL, Orbit, end_alt,
AtTargetUT,
Math.Min(LandingTrajectoryAutopilot.C.AtmoTrajectoryResolution,
Utils.ClampL((VSL.Altitude.Absolute - TargetAltitude) / Math.Abs(VSL.VerticalSpeed.Absolute) / 20, 0.1)),
VSL.Physics.M - ManeuverFuel);
update_overheat_info(Path, VSL.TCA.part.temperature);
AtTargetVel = Path.LastPoint.vel;
AtTargetPos = Path.LastPoint.pos;
AtTargetUT = Path.LastPoint.UT;
TransferTime = AtTargetUT - StartUT;
update_landing_site(Path);
}
void update(bool with_brake)
{
update_from_orbit(Orbit, StartUT);
//correct for brake maneuver
if (with_brake)
{
BrakeEndUT = AtTargetUT - GLB.LTRJ.CorrectionOffset;
BrakeStartUT = BrakeEndUT - MatchVelocityAutopilot.BrakingOffset((float)AtTargetVel.magnitude, VSL, out BrakeDuration);
brake_delta_v = -0.9 * Orbit.getOrbitalVelocityAtUT(BrakeEndUT);
update_from_orbit(TrajectoryCalculator.NewOrbit(Orbit, BrakeDeltaV, BrakeEndUT), BrakeEndUT);
}
else
{
brake_delta_v = -(AtTargetVel + Vector3d.Cross(Body.zUpAngularVelocity, AtTargetPos));
BrakeEndUT = TrajectoryCalculator.FlyAboveUT(Orbit, Target.RelSurfPos(Body).xzy, StartUT);
BrakeStartUT = BrakeEndUT - MatchVelocityAutopilot.BrakingOffset((float)BrakeDeltaV.magnitude, VSL, out BrakeDuration);
}
//compute vessel coordinates at maneuver start
if (VSL.LandedOrSplashed)
{
VslStartLat = Utils.ClampAngle(VSL.vessel.latitude);
VslStartLon = Utils.ClampAngle(VSL.vessel.longitude);
}
else
{
var start_pos = (TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToStart) * StartPos).xzy + Body.position;
VslStartLat = Utils.ClampAngle(Body.GetLatitude(start_pos));
VslStartLon = Utils.ClampAngle(Body.GetLongitude(start_pos));
}
//compute distance to target
DistanceToTarget = Target.AngleTo(SurfacePoint) * Body.Radius;
BrakeEndDeltaAlt = Orbit.getRelativePositionAtUT(BrakeEndUT).magnitude - Body.Radius - TargetAltitude;
//compute distance in lat-lon coordinates
DeltaLat = Utils.AngleDelta(SurfacePoint.Pos.Lat, Target.Pos.Lat) *
Math.Sign(Utils.AngleDelta(Utils.ClampAngle(VslStartLat), SurfacePoint.Pos.Lat));
DeltaLon = Utils.AngleDelta(SurfacePoint.Pos.Lon, Target.Pos.Lon) *
Math.Sign(Utils.AngleDelta(Utils.ClampAngle(VslStartLon), SurfacePoint.Pos.Lon));
//compute distance in radial coordinates
DeltaFi = 90 - Vector3d.Angle(Orbit.GetOrbitNormal(),
TrajectoryCalculator.BodyRotationAtdT(Body, TimeToSurface) *
Body.GetRelSurfacePosition(Target.Pos.Lat, Target.Pos.Lon, TargetAltitude).xzy);
DeltaR = Utils.RadDelta(SurfacePoint.AngleTo(VslStartLat, VslStartLon), Target.AngleTo(VslStartLat, VslStartLon)) * Mathf.Rad2Deg;
}
public Point FlyAbovePoint(Vector3d pos)
{
if (Utils.ProjectionAngle(Orbit.getRelativePositionAtUT(StartUT), pos,
Orbit.getOrbitalVelocityAtUT(StartUT)) < 0)
{
return(newP(StartUT));
}
if (!HavePoints ||
Utils.ProjectionAngle(Orbit.getRelativePositionAtUT(AtmoStartUT),
TrajectoryCalculator.BodyRotationAtdT(Orbit.referenceBody, AtmoStartUT - UT0) * pos,
Orbit.getOrbitalVelocityAtUT(AtmoStartUT)) < 0)
{
return(newP(TrajectoryCalculator.FlyAboveUT(Orbit, pos, StartUT)));
}
var p0 = Points[0];
var angle0 = Utils.ProjectionAngle(p0.pos,
TrajectoryCalculator
.BodyRotationAtdT(Orbit.referenceBody, p0.UT - UT0) * pos,
p0.vel);
for (int i = 1, count = Points.Count; i < count; i++)
{
var p1 = Points[i];
var angle1 = Utils.ProjectionAngle(p1.pos,
TrajectoryCalculator
.BodyRotationAtdT(Orbit.referenceBody, p1.UT - UT0) * pos,
p1.vel);
if (angle1 > 0)
{
angle0 = angle1;
continue;
}
p0 = Points[i - 1];
var p = p0;
var t = angle0 / (angle0 - angle1);
p.UT += p0.Duration * t;
p.Duration = p1.UT - p.UT;
p.pos = Vector3d.Lerp(p0.pos, p1.pos, t);
p.Update();
return(p);
}
return(LastPoint);
}
public void Update()
{
Altitude = pos.magnitude - Body.Radius;
Atmosphere = Body.atmosphere && Altitude < Body.atmosphereDepth;
rel_pos = Body.BodyFrame.WorldToLocal(TrajectoryCalculator.BodyRotationAtdT(Body, Path.UT0 - UT) * pos);
srf_vel = vel + Vector3d.Cross(Body.zUpAngularVelocity, pos);
HorSrfSpeed = Vector3d.Exclude(rel_pos, srf_vel).magnitude;
SrfSpeed = srf_vel.magnitude;
if (Atmosphere)
{
Pressure = Body.GetPressure(Altitude);
AtmosphereTemperature = Body.GetTemperature(Altitude);
Density = Body.GetDensity(Pressure, AtmosphereTemperature);
Mach1 = Body.GetSpeedOfSound(Pressure, Density);
var Rho_v = Density * SrfSpeed;
DynamicPressure = Rho_v * SrfSpeed / 2;
Mach = SrfSpeed / Mach1;
SpecificDrag = AtmoSim.Cd * DynamicPressure *
PhysicsGlobals.DragCurveValue(PhysicsGlobals.SurfaceCurves, VSL.OnPlanetParams.DragCurveK, (float)Mach) *
PhysicsGlobals.DragCurvePseudoReynolds.Evaluate((float)(Rho_v));
var convectiveMachLerp = Math.Pow(UtilMath.Clamp01((Mach - PhysicsGlobals.NewtonianMachTempLerpStartMach) /
(PhysicsGlobals.NewtonianMachTempLerpEndMach - PhysicsGlobals.NewtonianMachTempLerpStartMach)),
PhysicsGlobals.NewtonianMachTempLerpExponent);
ShockTemperature = SrfSpeed * PhysicsGlobals.NewtonianTemperatureFactor;
if (convectiveMachLerp > 0.0)
{
double b = PhysicsGlobals.MachTemperatureScalar * Math.Pow(SrfSpeed, PhysicsGlobals.MachTemperatureVelocityExponent);
ShockTemperature = UtilMath.LerpUnclamped(ShockTemperature, b, convectiveMachLerp);
}
ShockTemperature *= (double)HighLogic.CurrentGame.Parameters.Difficulty.ReentryHeatScale * Body.shockTemperatureMultiplier;
ShockTemperature = Math.Max(AtmosphereTemperature, ShockTemperature);
//calculate convective coefficient for speed > Mach1; lower speed is not a concern
ConvectiveCoefficient = 1E-10 * PhysicsGlobals.MachConvectionFactor;
ConvectiveCoefficient *= Density > 1 ? Density : Math.Pow(Density, PhysicsGlobals.MachConvectionDensityExponent);
ConvectiveCoefficient *= Math.Pow(SrfSpeed, PhysicsGlobals.MachConvectionVelocityExponent) * Body.convectionMultiplier;
}
}
void update()
{
if (TransferTime < 0)
{
TrajectoryCalculator.ClosestApproach(Orbit, TargetOrbit, StartUT, out AtTargetUT);
TransferTime = AtTargetUT - StartUT;
}
else
{
AtTargetUT = StartUT + TransferTime;
}
AtTargetPos = Orbit.getRelativePositionAtUT(AtTargetUT);
AtTargetVel = Orbit.getOrbitalVelocityAtUT(AtTargetUT);
TargetPos = TargetOrbit.getRelativePositionAtUT(AtTargetUT);
DistanceToTarget = Utils.ClampL((AtTargetPos - TargetPos).magnitude - VSL.Geometry.R - TargetVessel.Radius(), 0);
DeltaTA = Utils.ProjectionAngle(AtTargetPos, TargetPos,
Vector3d.Cross(Orbit.GetOrbitNormal(), AtTargetPos)) *
Math.Sign(TargetOrbit.period - OrigOrbit.period);
DeltaFi = TrajectoryCalculator.RelativeInclination(Orbit, TargetPos);
DeltaR = Vector3d.Dot(TargetPos - AtTargetPos, AtTargetPos.normalized);
KillerOrbit = Orbit.PeR < MinPeR && Orbit.timeToPe < TransferTime;
// DebugUtils.Log("{}", this);//debug
}
private bool update_maneuver_node()
{
Node = null;
NodeCB = null;
TargetOrbit = null;
if (Solver != null)
{
if (Solver.maneuverNodes.Count <= 0)
{
return(false);
}
Node = Solver.maneuverNodes[0];
}
else
{
if (VSL.vessel.flightPlanNode.nodes.Count <= 0)
{
return(false);
}
var node = VSL.vessel.flightPlanNode.nodes[0];
Node = new ManeuverNode();
Node.Load(node);
Node.patch = new Orbit(VSL.orbit);
Node.nextPatch =
TrajectoryCalculator.NewOrbit(VSL.orbit, Utils.Node2OrbitalDeltaV(Node), Node.UT);
VSL.vessel.flightPlanNode.RemoveNode(node);
}
NodeCB = Node.patch.referenceBody;
TargetOrbit = Node.nextPatch;
update_node_deltaV();
if (VSL.Engines.MaxDeltaV < Node.DeltaV.magnitude)
{
Status(Colors.Warning,
"WARNING: there may be not enough propellant for the maneuver");
}
return(true);
}
bool StartCondition(float dV)
{
if (Working)
{
return(true);
}
if (!CFG.AP1[Autopilot1.MatchVelNear])
{
return(true);
}
//calculate time to nearest approach
double ApprUT;
var tOrb = Target.GetOrbit();
TrajectoryCalculator.ClosestApproach(VSL.orbit, tOrb, VSL.Physics.UT, out ApprUT);
TTA = (float)(ApprUT - VSL.Physics.UT);
var ApprdV = (VSL.orbit.getOrbitalVelocityAtUT(ApprUT) - tOrb.getOrbitalVelocityAtUT(ApprUT)).xzy;
dV = (float)ApprdV.magnitude;
CFG.AT.OnIfNot(Attitude.Custom);
ATC.SetThrustDirW(ApprdV);
if (TTA > 0)
{
VSL.Info.Countdown = TTA - BrakingOffset(dV, VSL, out VSL.Info.TTB);
if (VSL.Controls.CanWarp)
{
VSL.Controls.WarpToTime = VSL.Physics.UT + VSL.Info.Countdown - VSL.Controls.MinAlignmentTime;
}
if (VSL.Info.Countdown > 0)
{
return(false);
}
}
VSL.Info.Countdown = 0;
Working = true;
return(true);
}
public Point PointAtAltitude(double altitude)
{
if (!HavePoints || altitude > Points[0].Altitude)
{
return(newP(TrajectoryCalculator.NearestRadiusUT(Orbit, altitude + Orbit.referenceBody.Radius, StartUT)));
}
for (int i = 1, count = Points.Count; i < count; i++)
{
var p1 = Points[i];
if (p1.Altitude > altitude)
{
continue;
}
var p0 = Points[i - 1];
var p = p0;
var t = (p0.Altitude - altitude) / (p0.Altitude - p1.Altitude);
p.UT += p0.Duration * t;
p.Duration = p1.UT - p.UT;
p.pos = Vector3d.Lerp(p0.pos, p1.pos, t);
p.Update();
return(p);
}
return(LastPoint);
}
protected BaseTrajectory(VesselWrapper vsl, Vector3d dV, double startUT)
{
VSL = vsl;
var dVm = (float)dV.magnitude;
ManeuverDeltaV = dV;
if (dVm > 0)
{
ManeuverFuel = VSL.Engines.FuelNeeded(dVm);
NotEnoughFuel = ManeuverFuel > VSL.Engines.GetAvailableFuelMass();
ManeuverDuration = VSL.Engines.TTB(dVm);
}
else
{
ManeuverFuel = 0;
ManeuverDuration = 0;
}
StartUT = startUT;
Body = VSL.vessel.orbitDriver.orbit.referenceBody;
OrigOrbit = VSL.vessel.orbitDriver.orbit;
Orbit = TrajectoryCalculator.NewOrbit(OrigOrbit, ManeuverDeltaV, StartUT);
StartPos = Orbit.getRelativePositionAtUT(StartUT);
StartVel = Orbit.getOrbitalVelocityAtUT(StartUT);
}
public LandingPath(VesselWrapper vsl, Orbit orb, double target_altitude, double startUT, double dt,
double start_mass, double fuel = 0, double brake_vel = 0)
{
VSL = vsl;
Orbit = orb;
TargetAltitude = target_altitude;
StartUT = startUT;
UT0 = VSL.Physics.UT;
EndMass = start_mass;
FuelUsed = 0;
FuelLeft = fuel;
BrakeDeltaV = brake_vel;
if (Orbit.referenceBody.atmosphere || brake_vel > 0 && fuel > 0)
{
EndUT = startUT + orb.timeToPe;
HavePoints = brake_vel > 0 && fuel > 0;
var p = newP(StartUT);
var m = start_mass;
var s = Math.Max(VSL.Geometry.MinArea, VSL.Geometry.AreaWithBrakes);
var ascending = p.Ascending;
var orig_dt = dt;
p.Duration = dt;
while ((ascending || p.Altitude > TargetAltitude) && p.UT < EndUT)
{
var dAlt = p.Altitude - TargetAltitude;
var linear_time = dAlt / p.SrfSpeed;
if (!ascending && dt > 0.01 && linear_time / 2 < dt)
{
p.Duration = dt = linear_time / 2;
}
var drag_accel = p.SpecificDrag * s / m;
var prev_alt = p.Altitude;
Atmosphere |= drag_accel > 0;
HavePoints |= Atmosphere;
if (HavePoints)
{
if (Points.Count == 0)
{
FirstPointUT = p.UT;
if (Atmosphere)
{
AtmoStartUT = p.UT;
}
}
Points.Add(p);
// VSL.Log("drag dV {}, m {}, fm {}, brake_vel {}, p {}",
// drag_dv, m, fuel, brake_vel, p);//debug
var r = p.pos.magnitude;
if (p.HorSrfSpeed > 1 && brake_vel > 0 && fuel > 0)
{
//adjust dt to capture most of the thrust
float mflow;
var thrust = VSL.Engines.ThrustAtAlt((float)p.SrfSpeed, (float)p.Altitude, out mflow);
var dV_cm = thrust / m * dt;
if (dt > 0.01 && dV_cm > p.SrfSpeed / 10)
{
dt = p.SrfSpeed / 10 * m / thrust * 0.9;
}
//compute thrust direction
var vV = Utils.ClampL(Vector3d.Dot(p.vel, p.pos / r), 1e-5);
var b_dir = LandingTrajectoryAutopilot.CorrectedBrakeVelocity(VSL, p.vel, p.pos,
p.DynamicPressure / 1000 / LandingTrajectoryAutopilot.C.MinDPressure,
(p.Altitude - TargetAltitude) / vV).normalized;
//compute change in velocity and mass
var Ve = thrust / mflow;
var dm = mflow * dt;
if (dm > fuel)
{
dm = fuel;
}
var bV = (double)VSL.Engines.DeltaV(Ve, (float)dm);
if (bV > brake_vel)
{
bV = brake_vel;
dm = EnginesProps.FuelNeeded((float)bV, Ve, (float)m);
}
p.vel -= b_dir * bV;
brake_vel -= bV;
// VSL.Log("vV {}, vB {}, thrust {}, mflow {}, Ve {}, dm {}\nb_dir {}\nvel {}\npos {}",
// vV, bV, thrust, mflow, Ve, dm, b_dir, p.vel, p.pos);//debug
//spend fuel
fuel -= dm;
m -= dm;
}
//adjust dt if its too small
else if (!ascending && dt < orig_dt && linear_time / 50 > dt)
{
dt = Math.Min(linear_time / 50, orig_dt);
}
if (Atmosphere)
{
p.vel -= p.srf_vel / p.SrfSpeed * Math.Min(drag_accel * dt, p.SrfSpeed);
}
p.vel -= p.pos * p.Body.gMagnitudeAtCenter / r / r / r * dt;
p.pos += p.vel * dt;
p.UT += dt;
p.Update();
if (Atmosphere && AtmoStopUT < 0 && p.SrfSpeed < 10)
{
AtmoStopUT = p.UT;
}
}
else
{
p.Update(p.UT + dt);
}
ascending &= p.Altitude > prev_alt;
}
if (HavePoints)
{
Points.Add(p);
if (Atmosphere && AtmoStopUT < 0)
{
AtmoStopUT = p.UT;
}
}
EndUT = p.UT;
LastPoint = p;
EndMass = m;
FuelUsed = start_mass - m;
FuelLeft = Math.Max(fuel, 0);
if (brake_vel > 0)
{
BrakeDeltaV -= brake_vel;
}
}
else
{
EndUT = TrajectoryCalculator.NearestRadiusUT(Orbit, Orbit.referenceBody.Radius + TargetAltitude, StartUT);
LastPoint = newP(EndUT);
}
}
void update(bool with_brake)
{
update_from_orbit(Orbit, StartUT);
LandingAngle = 90 - Vector3d.Angle(AtTargetPos, -AtTargetVel);
//correct for brake maneuver
if (with_brake)
{
//estimate time needed to rotate the ship downwards
var rotation_time = VSL.Torque.NoEngines?
VSL.Torque.NoEngines.MinRotationTime(90) :
VSL.Torque.MaxPossible.RotationTime(90, 0.1f);
//estimate amount fuel needed for the maneuver
var vertical_vel = Vector3d.Project(AtTargetVel, AtTargetPos);
SetBrakeEndUT(Math.Max(AtTargetUT - GLB.LTRJ.CorrectionOffset + rotation_time, StartUT));
SetBrakeDeltaV(vertical_vel);
if (BrakeFuel > 0)
{
//calculate braking maneuver
var dV = (float)BrakeDeltaV.magnitude;
BrakeDuration = VSL.Engines.AntigravTTB(dV);
BrakeDuration += rotation_time;
//find the appropriate point to perform the maneuver
var brake_end_UT = Math.Max(AtTargetUT - Mathf.Max(GLB.LTRJ.CorrectionOffset, BrakeDuration * 1.1f), StartUT);
var vertical_speed = vertical_vel.magnitude;
double fly_over_error;
do
{
SetBrakeEndUT(brake_end_UT);
fly_over_error = BrakeEndDeltaAlt - GLB.LTRJ.FlyOverAlt;
brake_end_UT -= Math.Abs(fly_over_error / vertical_speed);
} while(brake_end_UT > StartUT && fly_over_error < -1);
SetBrakeDeltaV(-0.9 * Orbit.getOrbitalVelocityAtUT(BrakeEndUT));
//calculate maneuver start time and update landing site
BrakeStartUT = Math.Max(BrakeEndUT - MatchVelocityAutopilot.BrakingOffset((float)BrakeDeltaV.magnitude, VSL, out BrakeDuration), StartUT);
update_from_orbit(TrajectoryCalculator.NewOrbit(Orbit, BrakeDeltaV, BrakeEndUT), BrakeEndUT);
}
else
{
BrakeStartUT = BrakeEndUT;
BrakeDuration = 0;
}
}
else
{
SetBrakeEndUT(TrajectoryCalculator.FlyAboveUT(Orbit, Target.RelOrbPos(Body), StartUT));
SetBrakeDeltaV(-(AtTargetVel + Vector3d.Cross(Body.zUpAngularVelocity, AtTargetPos)));
if (BrakeFuel > 0)
{
var offset = MatchVelocityAutopilot.BrakingOffset((float)BrakeDeltaV.magnitude, VSL, out BrakeDuration);
BrakeStartUT = Math.Max(BrakeEndUT - offset, StartUT);
}
else
{
BrakeStartUT = BrakeEndUT;
BrakeDuration = 0;
}
}
BrakeOffset = (float)Utils.ClampL(BrakeEndUT - BrakeStartUT, 0);
//compute vessel coordinates at maneuver start
if (VSL.LandedOrSplashed)
{
VslStartLat = Utils.ClampAngle(VSL.vessel.latitude);
VslStartLon = Utils.ClampAngle(VSL.vessel.longitude);
}
else
{
var start_pos = (TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToStart) * StartPos).xzy + Body.position;
VslStartLat = Utils.ClampAngle(Body.GetLatitude(start_pos));
VslStartLon = Utils.ClampAngle(Body.GetLongitude(start_pos));
}
//compute distance to target
DistanceToTarget = Target.AngleTo(SurfacePoint) * Body.Radius;
SurfacePoint.Name += string.Format("\n{0} from target", Utils.formatBigValue((float)DistanceToTarget, "m"));
//compute distance in lat-lon coordinates
DeltaLat = Utils.AngleDelta(SurfacePoint.Pos.Lat, Target.Pos.Lat) *
Math.Sign(Utils.AngleDelta(approach.Lat, SurfacePoint.Pos.Lat));
DeltaLon = Utils.AngleDelta(SurfacePoint.Pos.Lon, Target.Pos.Lon) *
Math.Sign(Utils.AngleDelta(approach.Lon, SurfacePoint.Pos.Lon));
//compute distance in radial coordinates
DeltaFi = 90 - Vector3d.Angle(Orbit.GetOrbitNormal(),
TrajectoryCalculator.BodyRotationAtdT(Body, TimeToTarget) *
Body.GetRelSurfacePosition(Target.Pos.Lat, Target.Pos.Lon, TargetAltitude).xzy);
DeltaR = Utils.RadDelta(SurfacePoint.AngleTo(VslStartLat, VslStartLon), Target.AngleTo(VslStartLat, VslStartLon)) * Mathf.Rad2Deg;
// Utils.Log("{}", this);//debug
}
public IEnumerable <double> FromSurfaceTTA(float ApA_offset, double ApA, double alpha, float maxG, float angularV)
{
//Log("FromSurfaceTTA: ApA_offset {}, ApA {}, alpha {}, maxG {}, angularV {}",
//ApA_offset, ApA, alpha*Mathf.Rad2Deg, maxG, angularV*Mathf.Rad2Deg);//debug
var t = 0.0;
var BR = Body.Radius;
var ApR = BR + ApA;
var v = new Vector2d(1e-3, 1e-3);
var r0n = new Vector2d(0, 1);
var r = new Vector2d(0, VSL.orbit.radius);
var r1n = new Vector2d(Math.Sin(alpha), Math.Cos(alpha));
var r1 = r1n * ApR;
var T = new Vector2d(0, 1);
var m = (double)VSL.Physics.M;
var m0 = m;
var mT = eStats.MaxThrust;
var mTm = mT.magnitude;
var mflow = eStats.MaxMassFlow;
var AA = eStats.TorqueInfo.AA_rad;
var s = VSL.Geometry.AreaInDirection(mT);
var thrust = true;
var R = r.magnitude;
var prev_r = r;
var maxR = ApR * 2;
double turn_start = VSL.Altitude.Absolute;
bool turn_started = false;
Orbit obt = null;
while (R > BR && R < maxR &&
Utils.Angle2(r0n, r1n) > Utils.Angle2(r0n, r))
{
yield return(-1);
prev_r = r;
R = r.magnitude;
var h = R - BR;
double time2ApA;
var ApV = getApV(m, s, r, v, C.DeltaTime * 4, out time2ApA);
thrust &= Vector2d.Dot(ApV - r1, r1 - r) < 0;
var srf_dir = -r.Rotate90().normalized;
var thr = thrust ? max_G_throttle((float)Vector2d.Dot(T, r.normalized), m, maxG) : 0;
var nV = v;
if (thrust &&
Vector2d.Dot(r.normalized, v) / VSL.Physics.StG > ToOrbitExecutor.C.MinClimbTime)
{
var rr1 = r1 - r;
solver.Init(Body, r, v, r1);
var minT = solver.ParabolicTime;
var maxT = solver.MinEnergyTime;
nV = solver.dV4TransferME();
while (maxT - minT > 0.1)
{
var curT = (maxT + minT) / 2;
nV = solver.dV4Transfer(curT);
obt = TrajectoryCalculator.NewOrbit(Body, r, v + nV, VSL.Physics.UT, obt);
if (obt.timeToAp > curT)
{
minT = curT;
}
else
{
maxT = curT;
}
}
var neededAoA = Utils.ClampH(Utils.Angle2(rr1, r) / 2, 45);
var angle2Hor = 90 - Utils.Angle2(nV, r);
var AoA = Utils.Angle2(r, v);
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);
}
if (!turn_started && h < Body.atmosphereDepth)
{
var atm = Body.AtmoParamsAtAltitude(h);
if (atm.Rho > ToOrbitExecutor.C.AtmDensityOffset)
{
turn_start = h;
}
else
{
turn_started = true;
}
}
var startF = Utils.Clamp((h - turn_start) / ApA / ToOrbitExecutor.C.GTurnOffset, 0, 1);
var nT = v.Rotate(pitch.Action * startF);
var atErr = Utils.Angle2Rad(r, T) - Utils.Angle2Rad(r, nT);
T = T.RotateRad(atErr /
Math.Max(C.DeltaTime,
eStats.TorqueInfo.RotationTime3Phase((float)Math.Abs(atErr * Mathf.Rad2Deg),
(float)(AA * m0 / m),
C.RotAccelPhase,
1)) *
C.DeltaTime)
.normalized;
if (Vector2d.Dot(T, r) < 0)
{
T = srf_dir;
}
throttle_correction.Update((float)angle2Hor);
throttle.Update(ApA_offset + throttle_correction.Action - (float)time2ApA);
thr = Utils.ClampH(0.5f + throttle, thr);
}
if (thrust && thr > 0)
{
if (!CheatOptions.InfinitePropellant)
{
var dm = mflow * thr * C.DeltaTime;
if (m < dm)
{
thrust = false;
}
else
{
m -= dm;
}
}
if (thrust)
{
v += T * mTm / m * thr * C.DeltaTime;
}
}
v -= r * Body.gMagnitudeAtCenter / R / R / R * C.DeltaTime;
if (h < Body.atmosphereDepth)
{
var vm = v.magnitude;
if (vm > 0)
{
v -= v / vm * drag(s, h, vm) / m * C.DeltaTime;
}
}
r += v * C.DeltaTime;
r1n = r1n.RotateRad(angularV * C.DeltaTime).normalized;
r1 = r1n * ApR;
t += C.DeltaTime;
//DebugUtils.CSV("ToOrbitSim.csv", t, r);//debug
//DebugUtils.CSV("ToOrbitSim.csv", t, r, v, rel_v, T*mTm/m*thr, h, m, thr, r1, nV);//debug
}
//Log("TimeToApA: {}", t);//debug
yield return(t);
}
void update(bool with_brake, bool brake_at_fly_above)
{
reset();
//update everything
update_from_orbit();
update_landing_steepness();
ActualLandingAngle = 90 - Utils.Angle2(AtTargetPos, -AtTargetVel);
var AerobrakeStartUT = Path.Atmosphere ? Path.AtmoStartUT : AtTargetUT - 1;
//correct for brake maneuver
if (with_brake)
{
//estimate time needed to rotate the ship downwards
var rotation_time = VSL.Torque.NoEngines ?
VSL.Torque.NoEngines.RotationTime2Phase(90) :
VSL.Torque.MaxPossible.RotationTime2Phase(90, 0.1f);
//estimate amount of fuel needed for the maneuver
var vertical_vel = Vector3d.Project(AtTargetVel, AtTargetPos);
SetBrakeEndUT(Math.Max(AtTargetUT - LandingTrajectoryAutopilot.C.CorrectionOffset + rotation_time, StartUT));
SetBrakeDeltaV(vertical_vel);
if (BrakeFuel > 0)
{
//calculate braking maneuver
BrakeDuration = VSL.Engines.OnPlanetTTB(BrakeDeltaV,
Orbit.getRelativePositionAtUT(BrakeEndPoint.UT),
(float)(BrakeEndPointDeltaAlt + TargetAltitude));
BrakeDuration += rotation_time;
if (FullBrake)
{
if (brake_at_fly_above)
{
FlyAbovePoint = Path.FlyAbovePoint(Target.OrbPos(Body));
if (WillOverheat)
{
SetBrakeEndPoint(Path.PointAtShipTemp(VSL.Physics.MinMaxTemperature - 100));
}
else
{
SetBrakeEndPoint(FlyAbovePoint);
}
}
else
{
//find appropriate point to perform the maneuver
var brake_end_UT = Math.Max(AtTargetUT - Mathf.Max(LandingTrajectoryAutopilot.C.CorrectionOffset, BrakeDuration * 1.1f), StartUT);
var fly_over_alt = TargetAltitude + LandingTrajectoryAutopilot.C.FlyOverAlt;
if (WillOverheat)
{
SetBrakeEndPoint(Path.PointAtShipTemp(VSL.Physics.MinMaxTemperature - 100));
}
else if (Path.UT2Altitude(brake_end_UT) < fly_over_alt)
{
SetBrakeEndPoint(Path.PointAtAltitude(fly_over_alt));
}
else
{
SetBrakeEndUT(brake_end_UT);
}
}
}
else
{
SetBrakeEndUT(AerobrakeStartUT);
}
//update landing site
update_landing_site_after_brake();
}
else //no brake maneuver
{
SetBrakeEndUT(AerobrakeStartUT);
BrakeStartPoint = BrakeEndPoint;
BrakeDuration = 0;
}
}
else
{
FlyAbovePoint = Path.FlyAbovePoint(Target.OrbPos(Body));
if (WillOverheat)
{
SetBrakeEndPoint(Path.PointAtShipTemp(VSL.Physics.MinMaxTemperature - 100));
}
else
{
SetBrakeEndPoint(FlyAbovePoint);
}
SetBrakeDeltaV(-AtTargetVel - Vector3d.Cross(Body.zUpAngularVelocity, AtTargetPos));
if (BrakeFuel > 0)
{
var offset = MatchVelocityAutopilot.BrakingOffset((float)BrakeDeltaV.magnitude, VSL, out BrakeDuration);
BrakeStartPoint = Path.PointAtUT(Math.Max(BrakeEndPoint.UT - offset, StartUT));
}
else
{
SetBrakeEndUT(AerobrakeStartUT);
BrakeStartPoint = BrakeStartPoint;
BrakeDuration = 0;
}
}
BrakeOffset = (float)Utils.ClampL(BrakeEndPoint.UT - BrakeStartPoint.UT, 0);
//compute vessel coordinates at maneuver start
if (VSL.LandedOrSplashed)
{
VslStartLat = Utils.ClampAngle(VSL.vessel.latitude);
VslStartLon = Utils.ClampAngle(VSL.vessel.longitude);
}
else
{
var start_pos = (TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToStart) * StartPos).xzy + Body.position;
VslStartLat = Utils.ClampAngle(Body.GetLatitude(start_pos));
VslStartLon = Utils.ClampAngle(Body.GetLongitude(start_pos));
}
//compute distance to target
DistanceToTarget = Target.AngleTo(SurfacePoint) * Body.Radius;
SurfacePoint.Name += string.Format("\n{0} from target", Utils.formatBigValue((float)DistanceToTarget, "m"));
//compute distance in lat-lon coordinates
DeltaLat = Utils.AngleDelta(SurfacePoint.Pos.Lat, Target.Pos.Lat) *
Math.Sign(Utils.AngleDelta(approach.Lat, SurfacePoint.Pos.Lat));
DeltaLon = Utils.AngleDelta(SurfacePoint.Pos.Lon, Target.Pos.Lon) *
Math.Sign(Utils.AngleDelta(approach.Lon, SurfacePoint.Pos.Lon));
//compute distance in radial coordinates
DeltaFi = 90 - Utils.Angle2(Orbit.GetOrbitNormal(),
TrajectoryCalculator.BodyRotationAtdT(Body, TimeToTarget) * Target.OrbPos(Body));
DeltaR = Utils.RadDelta(SurfacePoint.AngleTo(VslStartLat, VslStartLon), Target.AngleTo(VslStartLat, VslStartLon)) * Mathf.Rad2Deg;
// Utils.Log("{}", this);//debug
}
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 BaseTrajectory(VesselWrapper vsl, Vector3d dV, double startUT)
{
VSL = vsl;
var dVm = (float)dV.magnitude;
if (dVm > 0)
{
ManeuverFuel = VSL.Engines.FuelNeeded(dVm);
FullManeuver = ManeuverFuel < VSL.Engines.AvailableFuelMass;
ManeuverDuration = VSL.Engines.TTB_Precise(dVm);
}
else
{
ManeuverFuel = 0;
ManeuverDuration = 0;
FullManeuver = true;
}
StartUT = startUT;
OrigOrbit = VSL.vessel.orbitDriver.orbit;
var obt = StartOrbit;
Body = obt.referenceBody;
if (dVm > 0)
{
ManeuverDeltaV = dV;
NodeDeltaV = Utils.Orbital2NodeDeltaV(obt, ManeuverDeltaV, StartUT);
try
{
Orbit = TrajectoryCalculator.NewOrbit(obt, ManeuverDeltaV, StartUT);
var prev = Orbit;
for (int i = 0; i < 3; i++)
{
if (!PatchedConics.CalculatePatch(prev, prev.nextPatch ?? new Orbit(), prev.epoch, new PatchedConics.SolverParameters(), null))
{
break;
}
prev = prev.nextPatch;
}
Body = Orbit.referenceBody;
// if(Orbit.patchEndTransition != Orbit.PatchTransitionType.FINAL)//debug
// {
// Utils.Log("**************************************************************************************************");//debug
// RendezvousAutopilot.log_patches(Orbit, "Orbit");//deubg
// }
}
catch (ArithmeticException)
{
Orbit = OrigOrbit;
StartUT = VSL.Physics.UT;
ManeuverFuel = 0;
ManeuverDuration = 0;
ManeuverDeltaV = Vector3d.zero;
FullManeuver = true;
}
}
else
{
Orbit = OrigOrbit;
}
StartPos = obt.getRelativePositionAtUT(StartUT);
StartVel = obt.getOrbitalVelocityAtUT(StartUT);
AfterStartVel = Orbit.getOrbitalVelocityAtUT(StartUT);
}
public Orbit OrbitFromHere()
{
return(TrajectoryCalculator.NewOrbit(Body, pos, vel, UT));
}
