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; } }