/
LowDeorbitBurn.cs
141 lines (118 loc) · 6.82 KB
/
LowDeorbitBurn.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
using System;
using KSP.Localization;
using UnityEngine;
// FIXME: use a maneuver node
namespace MuMech
{
namespace Landing
{
public class LowDeorbitBurn : AutopilotStep
{
private bool _deorbitBurnTriggered;
private double _lowDeorbitBurnMaxThrottle;
private bool _lowDeorbitEndConditionSet;
private bool _lowDeorbitEndOnLandingSiteNearer;
private const double LOW_DEORBIT_BURN_TRIGGER_FACTOR = 2;
public LowDeorbitBurn(MechJebCore core) : base(core)
{
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (_deorbitBurnTriggered && Core.Attitude.attitudeAngleFromTarget() < 5)
{
Core.Thrust.TargetThrottle = Mathf.Clamp01((float)_lowDeorbitBurnMaxThrottle);
}
else
{
Core.Thrust.TargetThrottle = 0;
}
return this;
}
public override AutopilotStep OnFixedUpdate()
{
//Decide when we will start the deorbit burn:
double stoppingDistance = Math.Pow(VesselState.speedSurfaceHorizontal, 2) / (2 * VesselState.limitedMaxThrustAccel);
double triggerDistance = LOW_DEORBIT_BURN_TRIGGER_FACTOR * stoppingDistance;
double heightAboveTarget = VesselState.altitudeASL - Core.Landing.DecelerationEndAltitude();
if (triggerDistance < heightAboveTarget)
{
triggerDistance = heightAboveTarget;
}
//See if it's time to start the deorbit burn:
double rangeToTarget = Vector3d.Exclude(VesselState.up, Core.Target.GetPositionTargetPosition() - VesselState.CoM).magnitude;
if (!_deorbitBurnTriggered && rangeToTarget < triggerDistance)
{
if (!MuUtils.PhysicsRunning()) Core.Warp.MinimumWarp(true);
_deorbitBurnTriggered = true;
}
Status = Localizer.Format(_deorbitBurnTriggered
? "#MechJeb_LandingGuidance_Status11"
: //"Executing low deorbit burn"
"#MechJeb_LandingGuidance_Status12"); //"Moving to low deorbit burn point"
//Warp toward deorbit burn if it hasn't been triggerd yet:
if (!_deorbitBurnTriggered && Core.Node.Autowarp && rangeToTarget > 2 * triggerDistance)
Core.Warp.WarpRegularAtRate((float)(Orbit.period / 6));
if (rangeToTarget < triggerDistance && !MuUtils.PhysicsRunning()) Core.Warp.MinimumWarp();
//By default, thrust straight back at max throttle
Vector3d thrustDirection = -VesselState.surfaceVelocity.normalized;
_lowDeorbitBurnMaxThrottle = 1;
//If we are burning, we watch the predicted landing site and switch to the braking
//burn when the predicted landing site crosses the target. We also use the predictions
//to steer the predicted landing site toward the target
if (_deorbitBurnTriggered && Core.Landing.PredictionReady)
{
//angle slightly left or right to fix any cross-range error in the predicted landing site:
Vector3d horizontalToLandingSite = Vector3d.Exclude(VesselState.up, Core.Landing.LandingSite - VesselState.CoM).normalized;
Vector3d horizontalToTarget =
Vector3d.Exclude(VesselState.up, Core.Target.GetPositionTargetPosition() - VesselState.CoM).normalized;
const double ANGLE_GAIN = 4;
Vector3d angleCorrection = ANGLE_GAIN * (horizontalToTarget - horizontalToLandingSite);
if (angleCorrection.magnitude > 0.1) angleCorrection *= 0.1 / angleCorrection.magnitude;
thrustDirection = (thrustDirection + angleCorrection).normalized;
double rangeToLandingSite = Vector3d.Exclude(VesselState.up, Core.Landing.LandingSite - VesselState.CoM).magnitude;
double maxAllowedSpeed = Core.Landing.MaxAllowedSpeed();
if (!_lowDeorbitEndConditionSet &&
Vector3d.Distance(Core.Landing.LandingSite, VesselState.CoM) < MainBody.Radius + VesselState.altitudeASL)
{
_lowDeorbitEndOnLandingSiteNearer = rangeToLandingSite > rangeToTarget;
_lowDeorbitEndConditionSet = true;
}
_lowDeorbitBurnMaxThrottle = 1;
if (Orbit.PeA < 0)
{
if (rangeToLandingSite > rangeToTarget)
{
if (_lowDeorbitEndConditionSet && !_lowDeorbitEndOnLandingSiteNearer)
{
Core.Thrust.TargetThrottle = 0;
return new DecelerationBurn(Core);
}
double maxAllowedSpeedAfterDt = Core.Landing.MaxAllowedSpeedAfterDt(VesselState.deltaT);
double speedAfterDt = VesselState.speedSurface +
VesselState.deltaT * Vector3d.Dot(VesselState.gravityForce, VesselState.surfaceVelocity.normalized);
double throttleToMaintainLandingSite;
if (VesselState.speedSurface < maxAllowedSpeed) throttleToMaintainLandingSite = 0;
else
throttleToMaintainLandingSite =
(speedAfterDt - maxAllowedSpeedAfterDt) / (VesselState.deltaT * VesselState.maxThrustAccel);
_lowDeorbitBurnMaxThrottle = throttleToMaintainLandingSite + 1 * (rangeToLandingSite / rangeToTarget - 1) + 0.2;
}
else
{
if (_lowDeorbitEndConditionSet && _lowDeorbitEndOnLandingSiteNearer)
{
Core.Thrust.TargetThrottle = 0;
return new DecelerationBurn(Core);
}
_lowDeorbitBurnMaxThrottle = 0;
Status = Localizer.Format(
"#MechJeb_LandingGuidance_Status13"); //"Deorbit burn complete: waiting for the right moment to start braking"
}
}
}
Core.Attitude.attitudeTo(thrustDirection, AttitudeReference.INERTIAL, Core.Landing);
return this;
}
}
}
}