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