public float Update(float angleError, float angularVelocity, float maxAA)
{
var aaNorm = Utils.ClampH(maxAngularAcceleration / maxAA, 1);
var aaCap = Mathf.Min(maxAngularAcceleration, maxAA);
var tau = avFilter * TimeWarp.fixedDeltaTime;
avActionFilter.Tau = tau;
pid.setTau(tau);
var errorDecreases = angleError < 0 && angularVelocity > 0 ||
angleError > 0 && angularVelocity < 0;
if (Mathf.Abs(angleError) < angularErrorTolerance)
{
pid.Update(Utils.Clamp(angularVelocity / aaCap, -1, 1) * aaNorm);
}
else if (errorDecreases)
{
var accelToStop = angleError.Equals(0)
? 0
: angularVelocity * angularVelocity / 2 / angleError;
var accelToStopAbs = Mathf.Abs(accelToStop);
var decelerateThreshold = Mathf.Abs(pid.Action) < upperLowerActionThreshold
? decelerateThresholdUpper
: decelerateThresholdLower;
if (accelToStopAbs > decelerateThreshold * aaCap)
{
pid.Update(-Utils.Clamp(accelToStop, -aaCap, aaCap) / maxAA);
}
else if (accelToStopAbs < accelerateThreshold * aaCap &&
Math.Abs(angularVelocity) < maxAngularVelocity)
{
pid.Update(Utils.Clamp(angleError * angleErrorToActionP, -1, 1) * aaNorm);
}
else
{
pid.Update(0);
}
}
else
{
pid.Update(Utils.Clamp(angleError * angleErrorToActionP + angularVelocity / aaCap,
-1,
1)
* aaNorm);
}
var action = tau > 0 ? avActionFilter.Update(pid.Action) : pid.Action;
if (odFilter > 0)
{
action *= 1 - odFilter * OD.Update(pid.Action, TimeWarp.fixedDeltaTime);
}
// DebugUtils.CSV($"AxisAttitudeCascade-{name}.csv",
// Time.timeSinceLevelLoad,
// angleError,
// angularVelocity,
// pid.Action,
// action); //debug
return(-action);
}
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);
}
public float UpdateAV(float avError)
{
var tau = ATCB.avAction_Filter * TimeWarp.fixedDeltaTime;
avFilter.Tau = tau;
avPID.setTau(tau);
avPID.Update(avError);
avPID.Action = avFilter.Update(avPID.Action);
return(avPID.Action);
}
public bool GravityTurn(float Dtol)
{
UpdateTargetPosition();
VSL.Engines.ActivateNextStageOnFlameout();
update_state(Dtol);
var pg_vel = get_pg_vel();
if (!ErrorThreshold)
{
CFG.AT.OnIfNot(Attitude.Custom);
CircularizationOffset = -1;
tune_THR();
auto_ApA_offset();
var startF = getStartF();
var vel = pg_vel;
var AoA = Utils.Angle2(pg_vel, VesselOrbit.pos);
if (AoA < 45)
{
pitch.Update((float)AoA - Mathf.Lerp(45, 15, (float)(VSL.vessel.atmDensity - 0.1)));
vel = QuaternionD.AngleAxis(pitch * startF, Vector3d.Cross(target, VesselOrbit.pos)) * pg_vel;
}
vel = tune_needed_vel(vel, pg_vel, startF);
vel = Utils.ClampDirection(vel, pg_vel, (double)AttitudeControlBase.C.MaxAttitudeError);
throttle.Update(TimeToApA - (float)VesselOrbit.timeToAp);
THR.Throttle = Utils.Clamp(0.5f + throttle, MinThrottle / 100, max_G_throttle()) *
(float)Utils.ClampH(dApA / Dtol / VSL.Engines.TMR, 1);
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));
}
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));
}
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);
}
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;
}
}