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