forked from RemoteTechnologiesGroup/RemoteTech
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathFlightCore.cs
More file actions
251 lines (214 loc) · 10.4 KB
/
Copy pathFlightCore.cs
File metadata and controls
251 lines (214 loc) · 10.4 KB
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
using System;
using System.Linq;
using UnityEngine;
using RemoteTech.FlightComputer.Commands;
namespace RemoteTech.FlightComputer
{
public static class FlightCore
{
public static bool UseSas = true;
public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceFrame frame, FlightAttitude attitude, Quaternion extra)
{
var v = f.Vessel;
var forward = Vector3.zero;
var up = Vector3.zero;
bool ignoreRoll = false;
f.PIDController.setPIDParameters(FlightComputer.PIDKp, FlightComputer.PIDKi, FlightComputer.PIDKd);
switch (frame)
{
case ReferenceFrame.Orbit:
ignoreRoll = true;
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Surface:
ignoreRoll = true;
forward = v.GetSrfVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.North:
up = (v.mainBody.position - v.CoM);
forward = Vector3.ProjectOnPlane(v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM, up);
break;
case ReferenceFrame.Maneuver:
ignoreRoll = true;
if (f.Vessel.patchedConicSolver == null)//scenario: two vessels within physical range with FC attitude hold cmds. Unloaded one doesn't have solver instance
{
f.Vessel.AttachPatchedConicsSolver();
f.Vessel.patchedConicSolver.Update();
}
if (f.Vessel.patchedConicSolver.maneuverNodes.Count != 0)
{
forward = f.Vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit);
up = (v.mainBody.position - v.CoM);
}
else
{
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
break;
case ReferenceFrame.TargetVelocity:
// f.DelayedTarget may be any ITargetable, including a planet
// Velocity matching only makes sense for vessels and part modules
// Can test for Vessel but not PartModule, so instead test that it's not the third case (CelestialBody)
if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody))
{
forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
else
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
case ReferenceFrame.TargetParallel:
if (f.DelayedTarget != null) // either target vessel or celestial body
{
forward = f.DelayedTarget.GetTransform().position - v.CoM;
up = (v.mainBody.position - v.CoM);
}
else // no target to aim; default to orbital prograde
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
}
Vector3.OrthoNormalize(ref forward, ref up);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
switch (attitude)
{
case FlightAttitude.Prograde:
break;
case FlightAttitude.Retrograde:
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
break;
case FlightAttitude.NormalPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up);
break;
case FlightAttitude.NormalMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down);
break;
case FlightAttitude.RadialPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right);
break;
case FlightAttitude.RadialMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left);
break;
case FlightAttitude.Surface:
rotationReference = rotationReference * extra;
break;
}
HoldOrientation(fs, f, rotationReference, ignoreRoll);
}
public static void HoldOrientation(FlightCtrlState fs, FlightComputer f, Quaternion target, bool ignoreRoll = false)
{
f.Vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false);
SteeringHelper.SteerShipToward(target, fs, f, ignoreRoll);
}
/// <summary>
/// Checks the needed propellant of an engine. Its always true if infinite fuel is activ
/// </summary>
/// <param name="propellants">Propellant for an engine</param>
/// <returns>True if there are enough propellant to perform</returns>
public static bool hasPropellant(System.Collections.Generic.List<Propellant> propellants)
{
if (CheatOptions.InfinitePropellant) return true;
foreach (var props in propellants)
{
var total = props.totalResourceCapacity;
var require = props.currentRequirement;
// check the total capacity and the required amount of proppelant
if (total <= 0 || require > total)
{
return false;
}
}
return true;
}
/// <summary>
/// Get the total thrust of all activated, not flamed out engines.
/// </summary>
/// <param name="v">Current vessel</param>
/// <returns>Total thrust in kN</returns>
public static double GetTotalThrust(Vessel v)
{
double thrust = 0.0;
foreach (var pm in v.parts.SelectMany(p => p.FindModulesImplementing<ModuleEngines>()))
{
// Notice: flameout is only true if you try to perform with this engine not before
if (!pm.EngineIgnited || pm.flameout) continue;
// check for the needed propellant before changing the total thrust
if (!FlightCore.hasPropellant(pm.propellants)) continue;
thrust += (double)pm.maxThrust * (pm.thrustPercentage / 100);
}
return thrust;
}
}
public static class SteeringHelper
{
private const double outputDeadband = 0.001;
private const float driveLimit = 1.0f;
/// <summary>
/// Automatically guides the ship to face a desired orientation
/// </summary>
/// <param name="target">The desired orientation</param>
/// <param name="c">The FlightCtrlState for the current vessel.</param>
/// <param name="fc">The flight computer carrying out the slew</param>
/// <param name="ignoreRoll">[optional] to ignore the roll</param>
public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll)
{
var actuation = fc.PIDController.GetActuation(target);
// deadband
actuation.x = Math.Abs(actuation.x) >= outputDeadband ? actuation.x : 0.0;
actuation.y = Math.Abs(actuation.y) >= outputDeadband ? actuation.y : 0.0;
actuation.z = Math.Abs(actuation.z) >= outputDeadband ? actuation.z : 0.0;
// update the flight controls
c.pitch = Mathf.Clamp((float) actuation.x, -driveLimit, driveLimit);
c.roll = !ignoreRoll ? Mathf.Clamp((float) actuation.y, -driveLimit, driveLimit) : 0.0f;
c.yaw = Mathf.Clamp((float) actuation.z, -driveLimit, driveLimit);
}
/// <summary>
/// Get the total torque for a vessel.
/// </summary>
/// <param name="vessel">The vessel from which ot get the total torque.</param>
/// <returns>The vessel torque as a Vector3.</returns>
public static Vector3 GetVesselTorque(Vessel vessel)
{
// the resulting torque
Vector3 vesselTorque = Vector3.zero;
// positive and negative vessel torque for all part modules that are torque providers
Vector3 positiveTorque = Vector3.zero;
Vector3 negativeTorque = Vector3.zero;
// cycle through all vessel parts.
int partCount = vessel.Parts.Count;
for(int iPart = 0; iPart < partCount; ++iPart)
{
Part part = vessel.Parts[iPart];
// loop through all modules for the part
int moduleCount = part.Modules.Count;
for (int iModule = 0; iModule < moduleCount; ++iModule)
{
// find modules in part that are torque providers.
ITorqueProvider torqueProvider = part.Modules[iModule] as ITorqueProvider;
if (torqueProvider == null)
continue;
// pos and neg torque for this part module
Vector3 posTorque;
Vector3 negTorque;
// get potential torque for the current module and update pos and neg torques.
torqueProvider.GetPotentialTorque(out posTorque, out negTorque);
positiveTorque += posTorque;
negativeTorque += negTorque;
}
}
// get max torque from all components of pos and neg torques.
vesselTorque.x = Mathf.Max(positiveTorque.x, negativeTorque.x);
vesselTorque.y = Mathf.Max(positiveTorque.y, negativeTorque.y);
vesselTorque.z = Mathf.Max(positiveTorque.z, negativeTorque.z);
return vesselTorque;
}
}
}