/
MechJebModuleRendezvousAutopilot.cs
255 lines (219 loc) · 13.2 KB
/
MechJebModuleRendezvousAutopilot.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
using System;
using JetBrains.Annotations;
using KSP.Localization;
using static MechJebLib.Statics;
namespace MuMech
{
[UsedImplicitly]
public class MechJebModuleRendezvousAutopilot : ComputerModule
{
public MechJebModuleRendezvousAutopilot(MechJebCore core) : base(core) { }
[Persistent(pass = (int)Pass.GLOBAL)]
public EditableDouble desiredDistance = 100;
[Persistent(pass = (int)Pass.GLOBAL)]
public EditableDouble maxPhasingOrbits = 5;
[Persistent(pass = (int)Pass.GLOBAL)]
public EditableDouble maxClosingSpeed = 100;
public string status = "";
protected override void OnModuleEnabled()
{
Vessel.RemoveAllManeuverNodes();
if (!MuUtils.PhysicsRunning()) Core.Warp.MinimumWarp();
}
protected override void OnModuleDisabled() => Core.Node.Abort(); //make sure we turn off node executor if we get disabled suddenly
public override void Drive(FlightCtrlState s)
{
if (!Core.Target.NormalTargetExists)
{
Users.Clear();
return;
}
Core.Node.Autowarp = Core.Node.Autowarp && Core.Target.Distance > 1000;
//If we get within the target distance and then next maneuver node is still
//far in the future, delete it and we will create a new one to match velocities immediately.
//This can often happen because the target vessel's orbit shifts slightly when it is unpacked.
if (Core.Target.Distance < desiredDistance
&& Vessel.patchedConicSolver.maneuverNodes.Count > 0
&& Vessel.patchedConicSolver.maneuverNodes[0].UT > VesselState.time + 1)
{
Vessel.RemoveAllManeuverNodes();
}
if (Vessel.patchedConicSolver.maneuverNodes.Count > 0)
{
//If we have plotted a maneuver, execute it.
if (!Core.Node.Enabled) Core.Node.ExecuteAllNodes(this);
}
else if (Core.Target.Distance < desiredDistance * 1.05 + 2
&& Core.Target.RelativeVelocity.magnitude < 1)
{
//finished
Users.Clear();
Core.Thrust.ThrustOff();
status = Localizer.Format("#MechJeb_RZauto_statu1"); //"Successful rendezvous"
}
else if (Core.Target.Distance < desiredDistance * 1.05 + 2)
{
//We are within the target distance: match velocities
double UT = VesselState.time;
Vector3d dV = OrbitalManeuverCalculator.DeltaVToMatchVelocities(Orbit, UT, Core.Target.TargetOrbit);
Vessel.PlaceManeuverNode(Orbit, dV, UT);
status = Localizer.Format("#MechJeb_RZauto_statu2", desiredDistance.ToString()); //"Within " + + "m: matching velocities."
}
else if (Core.Target.Distance < VesselState.radius / 25)
{
if (Orbit.NextClosestApproachDistance(Core.Target.TargetOrbit, VesselState.time) < desiredDistance
&& Orbit.NextClosestApproachTime(Core.Target.TargetOrbit, VesselState.time) < VesselState.time + 150)
{
//We're close to the target, and on a course that will take us closer. Kill relvel at closest approach
double UT = Orbit.NextClosestApproachTime(Core.Target.TargetOrbit, VesselState.time);
Vector3d dV = OrbitalManeuverCalculator.DeltaVToMatchVelocities(Orbit, UT, Core.Target.TargetOrbit);
//adjust burn time so as to come to rest at the desired distance from the target:
double approachDistance = Orbit.Separation(Core.Target.TargetOrbit, UT);
double approachSpeed = (Orbit.WorldOrbitalVelocityAtUT(UT) - Core.Target.TargetOrbit.WorldOrbitalVelocityAtUT(UT)).magnitude;
if (approachDistance < desiredDistance)
{
UT -= Math.Sqrt(Math.Abs(desiredDistance * desiredDistance - approachDistance * approachDistance)) / approachSpeed;
}
//if coming in hot, stop early to avoid crashing:
if (approachSpeed > 10) UT -= 1;
Vessel.PlaceManeuverNode(Orbit, dV, UT);
status = Localizer.Format("#MechJeb_RZauto_statu3"); //"Planning to match velocities at closest approach."
}
else
{
//We're not far from the target. Close the distance
double closingSpeed = Core.Target.Distance / 100;
if (closingSpeed > maxClosingSpeed) closingSpeed = maxClosingSpeed;
closingSpeed = Math.Max(0.01, closingSpeed);
double closingTime = Core.Target.Distance / closingSpeed;
double UT = VesselState.time + 15;
(Vector3d dV, _) = OrbitalManeuverCalculator.DeltaVToInterceptAtTime(Orbit, UT, Core.Target.TargetOrbit, closingTime);
Vessel.PlaceManeuverNode(Orbit, dV, UT);
status = Localizer.Format("#MechJeb_RZauto_statu4"); //"Close to target: plotting intercept"
}
}
else if (Orbit.NextClosestApproachDistance(Core.Target.TargetOrbit, VesselState.time) < Core.Target.TargetOrbit.semiMajorAxis / 25)
{
//We're not close to the target, but we're on an approximate intercept course.
//Kill relative velocities at closest approach
double UT = Orbit.NextClosestApproachTime(Core.Target.TargetOrbit, VesselState.time);
Vector3d dV = OrbitalManeuverCalculator.DeltaVToMatchVelocities(Orbit, UT, Core.Target.TargetOrbit);
//adjust burn time so as to come to rest at the desired distance from the target:
double approachDistance = (Orbit.WorldPositionAtUT(UT) - Core.Target.TargetOrbit.WorldPositionAtUT(UT)).magnitude;
double approachSpeed = (Orbit.WorldOrbitalVelocityAtUT(UT) - Core.Target.TargetOrbit.WorldOrbitalVelocityAtUT(UT)).magnitude;
if (approachDistance < desiredDistance)
{
UT -= Math.Sqrt(Math.Abs(desiredDistance * desiredDistance - approachDistance * approachDistance)) / approachSpeed;
}
//if coming in hot, stop early to avoid crashing:
if (approachSpeed > 10) UT -= 1;
Vessel.PlaceManeuverNode(Orbit, dV, UT);
status = Localizer.Format("#MechJeb_RZauto_statu5"); //"On intercept course. Planning to match velocities at closest approach."
}
else if (Orbit.RelativeInclination(Core.Target.TargetOrbit) < 0.05 && Orbit.eccentricity < 0.05)
{
//We're not on an intercept course, but we have a circular orbit in the right plane.
(Vector3d hohmannDV, double hohmannUT, _, _) =
OrbitalManeuverCalculator.DeltaVAndTimeForHohmannTransfer(Orbit, Core.Target.TargetOrbit, VesselState.time, coplanar: false);
double numPhasingOrbits = (hohmannUT - VesselState.time) / Orbit.period;
double actualMaxPhasingOrbits = Math.Max(maxPhasingOrbits, 5); // ignore input values that are unreasonably small
if (numPhasingOrbits < actualMaxPhasingOrbits)
{
//It won't be too long until the intercept window. Plot a Hohmann transfer intercept.
Vessel.PlaceManeuverNode(Orbit, hohmannDV, hohmannUT);
status = Localizer.Format("#MechJeb_RZauto_statu6",
numPhasingOrbits.ToString("F2")); //"Planning Hohmann transfer for intercept after " + + " phasing orbits."
}
else
{
//We are in a circular orbit in the right plane, but we aren't phasing quickly enough. Move to a better phasing orbit
double axisRatio = Math.Pow(1 + 1.25 / actualMaxPhasingOrbits, 2.0 / 3.0);
double lowPhasingRadius = Core.Target.TargetOrbit.semiMajorAxis / axisRatio;
double highPhasingRadius = Core.Target.TargetOrbit.semiMajorAxis * axisRatio;
bool useLowPhasingRadius = lowPhasingRadius > MainBody.Radius + MainBody.RealMaxAtmosphereAltitude() + 3000 &&
Orbit.semiMajorAxis < Core.Target.TargetOrbit.semiMajorAxis;
double phasingOrbitRadius = useLowPhasingRadius ? lowPhasingRadius : highPhasingRadius;
if (Orbit.ApR < phasingOrbitRadius)
{
double UT1 = VesselState.time + 15;
Vector3d dV1 = OrbitalManeuverCalculator.DeltaVToChangeApoapsis(Orbit, UT1, phasingOrbitRadius);
Vessel.PlaceManeuverNode(Orbit, dV1, UT1);
Orbit transferOrbit = Vessel.patchedConicSolver.maneuverNodes[0].nextPatch;
double UT2 = transferOrbit.NextApoapsisTime(UT1);
Vector3d dV2 = OrbitalManeuverCalculator.DeltaVToCircularize(transferOrbit, UT2);
Vessel.PlaceManeuverNode(transferOrbit, dV2, UT2);
}
else if (Orbit.PeR > phasingOrbitRadius)
{
double UT1 = VesselState.time + 15;
Vector3d dV1 = OrbitalManeuverCalculator.DeltaVToChangePeriapsis(Orbit, UT1, phasingOrbitRadius);
Vessel.PlaceManeuverNode(Orbit, dV1, UT1);
Orbit transferOrbit = Vessel.patchedConicSolver.maneuverNodes[0].nextPatch;
double UT2 = transferOrbit.NextPeriapsisTime(UT1);
Vector3d dV2 = OrbitalManeuverCalculator.DeltaVToCircularize(transferOrbit, UT2);
Vessel.PlaceManeuverNode(transferOrbit, dV2, UT2);
}
else
{
double UT = Orbit.NextTimeOfRadius(VesselState.time, phasingOrbitRadius);
Vector3d dV = OrbitalManeuverCalculator.DeltaVToCircularize(Orbit, UT);
Vessel.PlaceManeuverNode(Orbit, dV, UT);
}
status = Localizer.Format("#MechJeb_RZauto_statu7", numPhasingOrbits.ToString("F1"), maxPhasingOrbits.text,
(phasingOrbitRadius - MainBody.Radius)
.ToSI(0)); //"Next intercept window would be <<1>> orbits away, which is more than the maximum of <<2>> phasing orbits. Increasing phasing rate by establishing new phasing orbit at <<3>>m
}
}
else if (Orbit.RelativeInclination(Core.Target.TargetOrbit) < 0.05)
{
//We're not on an intercept course. We're in the right plane, but our orbit isn't circular. Circularize.
bool circularizeAtPe;
if (Orbit.eccentricity > 1) circularizeAtPe = true;
else
circularizeAtPe = Math.Abs(Orbit.PeR - Core.Target.TargetOrbit.semiMajorAxis) <
Math.Abs(Orbit.ApR - Core.Target.TargetOrbit.semiMajorAxis);
double UT;
if (circularizeAtPe) UT = Math.Max(VesselState.time, Orbit.NextPeriapsisTime(VesselState.time));
else UT = Orbit.NextApoapsisTime(VesselState.time);
Vector3d dV = OrbitalManeuverCalculator.DeltaVToCircularize(Orbit, UT);
Vessel.PlaceManeuverNode(Orbit, dV, UT);
status = Localizer.Format("#MechJeb_RZauto_statu8"); //"Circularizing."
}
else
{
//We're not on an intercept course, and we're not in the right plane. Match planes
bool ascending;
if (Orbit.eccentricity < 1)
{
if (Orbit.TimeOfAscendingNode(Core.Target.TargetOrbit, VesselState.time) <
Orbit.TimeOfDescendingNode(Core.Target.TargetOrbit, VesselState.time))
{
ascending = true;
}
else
{
ascending = false;
}
}
else
{
if (Orbit.AscendingNodeExists(Core.Target.TargetOrbit))
{
ascending = true;
}
else
{
ascending = false;
}
}
double UT;
Vector3d dV;
if (ascending)
dV = OrbitalManeuverCalculator.DeltaVAndTimeToMatchPlanesAscending(Orbit, Core.Target.TargetOrbit, VesselState.time, out UT);
else dV = OrbitalManeuverCalculator.DeltaVAndTimeToMatchPlanesDescending(Orbit, Core.Target.TargetOrbit, VesselState.time, out UT);
Vessel.PlaceManeuverNode(Orbit, dV, UT);
status = Localizer.Format("#MechJeb_RZauto_statu9"); //"Matching planes."
}
}
}
}