Skip to content

Commit d3a598d

Browse files
author
Chris Woerz
committed
Flight code review changes
1 parent e7b9fd2 commit d3a598d

File tree

1 file changed

+57
-131
lines changed

1 file changed

+57
-131
lines changed

‎src/RemoteTech2/FlightComputer/FlightCore.cs‎

Lines changed: 57 additions & 131 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,5 @@
11
using System;
2-
using System.Collections.Generic;
32
using System.Linq;
4-
using System.Text;
53
using UnityEngine;
64

75
namespace RemoteTech
@@ -13,25 +11,26 @@ public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceF
1311
var v = f.Vessel;
1412
var forward = Vector3.zero;
1513
var up = Vector3.zero;
16-
var rotationReference = Quaternion.identity;
1714
switch (frame)
1815
{
1916
case ReferenceFrame.Orbit:
2017
forward = v.GetObtVelocity();
2118
up = (v.mainBody.position - v.CoM);
2219
break;
20+
2321
case ReferenceFrame.Surface:
2422
forward = v.GetSrfVelocity();
2523
up = (v.mainBody.position - v.CoM);
2624
break;
25+
2726
case ReferenceFrame.North:
2827
up = (v.mainBody.position - v.CoM);
2928
forward = Vector3.Exclude(up,
3029
v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM
3130
);
3231
break;
32+
3333
case ReferenceFrame.Maneuver:
34-
up = v.transform.up;
3534
if (f.DelayedManeuver != null)
3635
{
3736
forward = f.DelayedManeuver.GetBurnVector(v.orbit);
@@ -43,8 +42,9 @@ public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceF
4342
up = (v.mainBody.position - v.CoM);
4443
}
4544
break;
45+
4646
case ReferenceFrame.TargetVelocity:
47-
if (f.DelayedTarget != null && f.DelayedTarget is Vessel)
47+
if (f.DelayedTarget is Vessel)
4848
{
4949
forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity();
5050
up = (v.mainBody.position - v.CoM);
@@ -55,8 +55,9 @@ public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceF
5555
forward = v.GetObtVelocity();
5656
}
5757
break;
58+
5859
case ReferenceFrame.TargetParallel:
59-
if (f.DelayedTarget != null && f.DelayedTarget is Vessel)
60+
if (f.DelayedTarget is Vessel)
6061
{
6162
forward = f.DelayedTarget.GetTransform().position - v.CoM;
6263
up = (v.mainBody.position - v.CoM);
@@ -69,26 +70,32 @@ public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceF
6970
break;
7071
}
7172
Vector3.OrthoNormalize(ref forward, ref up);
72-
rotationReference = Quaternion.LookRotation(forward, up);
73+
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
7374
switch (attitude)
7475
{
7576
case FlightAttitude.Prograde:
7677
break;
78+
7779
case FlightAttitude.Retrograde:
7880
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
7981
break;
82+
8083
case FlightAttitude.NormalPlus:
8184
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up);
8285
break;
86+
8387
case FlightAttitude.NormalMinus:
8488
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down);
8589
break;
90+
8691
case FlightAttitude.RadialPlus:
8792
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right);
8893
break;
94+
8995
case FlightAttitude.RadialMinus:
9096
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left);
9197
break;
98+
9299
case FlightAttitude.Surface:
93100
rotationReference = rotationReference * extra;
94101
break;
@@ -104,32 +111,21 @@ public static void HoldOrientation(FlightCtrlState fs, FlightComputer f, Quatern
104111

105112
public static double GetTotalThrust(Vessel v)
106113
{
107-
double thrust = 0.0;
108-
// @todo: Is it possible to select ModuleEngines OR ModuleEnginesFX in a single iterator?
109-
foreach (var pm in v.parts.SelectMany(p => p.FindModulesImplementing<ModuleEngines>()))
110-
{
111-
if (!pm.EngineIgnited) continue;
112-
thrust += (double)pm.maxThrust * (pm.thrustPercentage / 100);
113-
}
114-
foreach (var pm in v.parts.SelectMany(p => p.FindModulesImplementing<ModuleEnginesFX>()))
115-
{
116-
if (!pm.EngineIgnited) continue;
117-
thrust += (double)pm.maxThrust * (pm.thrustPercentage / 100);
118-
}
114+
var thrust = v.parts.SelectMany(p => p.FindModulesImplementing<ModuleEngines>())
115+
.Where(pm => pm.EngineIgnited)
116+
.Sum(pm => (double)pm.maxThrust * (pm.thrustPercentage / 100));
117+
thrust += v.parts.SelectMany(p => p.FindModulesImplementing<ModuleEnginesFX>())
118+
.Where(pm => pm.EngineIgnited)
119+
.Sum(pm => (double)pm.maxThrust * (pm.thrustPercentage / 100));
119120
return thrust;
120121
}
121122
}
122-
123123
}
124124

125125
namespace kOS
126126
{
127127
public static class SteeringHelper
128128
{
129-
public static Vector3d prev_err;
130-
public static Vector3d integral;
131-
private static Vector3d[] averagedAct = new Vector3d[5];
132-
133129
public static void KillRotation(FlightCtrlState c, Vessel vessel)
134130
{
135131
var act = vessel.transform.InverseTransformDirection(vessel.rigidbody.angularVelocity).normalized;
@@ -144,16 +140,12 @@ public static void KillRotation(FlightCtrlState c, Vessel vessel)
144140
public static void SteerShipToward(Quaternion target, FlightCtrlState c, Vessel vessel)
145141
{
146142
// I take no credit for this, this is a stripped down, rearranged version of MechJeb's attitude control system
147-
148-
var CoM = vessel.findWorldCenterOfMass();
149-
var MoI = vessel.findLocalMOI(CoM);
150-
var mass = vessel.GetTotalMass();
151-
var up = (CoM - vessel.mainBody.position).normalized;
143+
var centerOfMass = vessel.findWorldCenterOfMass();
144+
var momentOfInertia = vessel.findLocalMOI(centerOfMass);
152145

153146
var vesselR = vessel.transform.rotation;
154147

155-
Quaternion delta;
156-
delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselR) * target);
148+
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselR) * target);
157149

158150
Vector3d deltaEuler = ReduceAngles(delta.eulerAngles);
159151
deltaEuler.y *= -1;
@@ -163,65 +155,19 @@ public static void SteerShipToward(Quaternion target, FlightCtrlState c, Vessel
163155

164156
Vector3d err = deltaEuler * Math.PI / 180.0F;
165157
err += new Vector3d(inertia.x, inertia.z, inertia.y);
166-
//err.Scale(SwapYZ(Vector3d.Scale(MoI, Inverse(torque))));
167-
168-
prev_err = err;
169158

170159
Vector3d act = 120.0f * err;
171160

172-
float precision = Mathf.Clamp((float)torque.x * 20f / MoI.magnitude, 0.5f, 10f);
173-
float drive_limit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
174-
175-
act.x = Mathf.Clamp((float)act.x, -drive_limit, drive_limit);
176-
act.y = Mathf.Clamp((float)act.y, -drive_limit, drive_limit);
177-
act.z = Mathf.Clamp((float)act.z, -drive_limit, drive_limit);
178-
179-
//act = averageVector3d(averagedAct, act, 2);
180-
181-
c.roll = Mathf.Clamp((float)(c.roll + act.z), -drive_limit, drive_limit);
182-
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -drive_limit, drive_limit);
183-
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -drive_limit, drive_limit);
161+
float precision = Mathf.Clamp((float)torque.x * 20f / momentOfInertia.magnitude, 0.5f, 10f);
162+
float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
184163

185-
/*
186-
// This revised version from 0.6 gave people problems with gravity turns. I've reverted but may try to make it work
187-
188-
var CoM = vessel.findWorldCenterOfMass();
189-
var MoI = vessel.findLocalMOI(CoM);
190-
var mass = vessel.GetTotalMass();
191-
var up = (CoM - vessel.mainBody.position).normalized;
164+
act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit);
165+
act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit);
166+
act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit);
192167

193-
var target = targetDir.Rotation;
194-
var vesselR = vessel.transform.rotation;
195-
196-
Quaternion delta;
197-
delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselR) * target);
198-
199-
Vector3d deltaEuler = ReduceAngles(delta.eulerAngles);
200-
deltaEuler.y *= -1;
201-
202-
Vector3d torque = GetTorque(vessel, c.mainThrottle);
203-
Vector3d inertia = GetEffectiveInertia(vessel, torque);
204-
205-
Vector3d err = deltaEuler * Math.PI / 180.0F;
206-
err += SwapYZ(inertia * 8);
207-
err.Scale(SwapYZ(Vector3d.Scale(MoI * 3, Inverse(torque))));
208-
209-
prev_err = err;
210-
211-
Vector3d act = 400.0f * err;
212-
213-
float precision = Mathf.Clamp((float)torque.x * 20f / MoI.magnitude, 0.5f, 10f);
214-
float drive_limit = Mathf.Clamp01((float)(err.magnitude * 450.0f / precision));
215-
216-
act.x = Mathf.Clamp((float)act.x, -drive_limit, drive_limit);
217-
act.y = Mathf.Clamp((float)act.y, -drive_limit, drive_limit);
218-
act.z = Mathf.Clamp((float)act.z, -drive_limit, drive_limit);
219-
220-
//act = averageVector3d(averagedAct, act, 2);
221-
222-
c.roll = Mathf.Clamp((float)(c.roll + act.z), -drive_limit, drive_limit);
223-
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -drive_limit, drive_limit);
224-
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -drive_limit, drive_limit);*/
168+
c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit);
169+
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit);
170+
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit);
225171
}
226172

227173
public static Vector3d SwapYZ(Vector3d input)
@@ -236,15 +182,15 @@ public static Vector3d Pow(Vector3d v3d, float exponent)
236182

237183
public static Vector3d GetEffectiveInertia(Vessel vessel, Vector3d torque)
238184
{
239-
var CoM = vessel.findWorldCenterOfMass();
240-
var MoI = vessel.findLocalMOI(CoM);
185+
var centerOfMass = vessel.findWorldCenterOfMass();
186+
var momentOfInertia = vessel.findLocalMOI(centerOfMass);
241187
var angularVelocity = Quaternion.Inverse(vessel.transform.rotation) * vessel.rigidbody.angularVelocity;
242-
var angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
188+
var angularMomentum = new Vector3d(angularVelocity.x * momentOfInertia.x, angularVelocity.y * momentOfInertia.y, angularVelocity.z * momentOfInertia.z);
243189

244190
var retVar = Vector3d.Scale
245191
(
246192
Sign(angularMomentum) * 2.0f,
247-
Vector3d.Scale(Pow(angularMomentum, 2), Inverse(Vector3d.Scale(torque, MoI)))
193+
Vector3d.Scale(Pow(angularMomentum, 2), Inverse(Vector3d.Scale(torque, momentOfInertia)))
248194
);
249195

250196
retVar.y *= 10;
@@ -255,7 +201,7 @@ public static Vector3d GetEffectiveInertia(Vessel vessel, Vector3d torque)
255201
public static Vector3d GetTorque(Vessel vessel, float thrust)
256202
{
257203
// Do everything in vessel coordinates
258-
var CoM = vessel.findLocalCenterOfMass();
204+
var centerOfMass = vessel.findLocalCenterOfMass();
259205

260206
// Don't assume any particular symmetry for the vessel
261207
float pitch = 0, roll = 0, yaw = 0;
@@ -267,70 +213,69 @@ public static Vector3d GetTorque(Vessel vessel, float thrust)
267213
if (!module.isEnabled)
268214
continue;
269215

270-
if (module is ModuleReactionWheel
271-
&& ((ModuleReactionWheel)module).wheelState == ModuleReactionWheel.WheelState.Active)
216+
var reactionWheelModule = module as ModuleReactionWheel;
217+
var rcsModule = module as ModuleRCS;
218+
if (reactionWheelModule != null && reactionWheelModule.wheelState == ModuleReactionWheel.WheelState.Active)
272219
{
273-
pitch += ((ModuleReactionWheel)module).PitchTorque;
274-
roll += ((ModuleReactionWheel)module).RollTorque;
275-
yaw += ((ModuleReactionWheel)module).YawTorque;
220+
pitch += reactionWheelModule.PitchTorque;
221+
roll += reactionWheelModule.RollTorque;
222+
yaw += reactionWheelModule.YawTorque;
276223
}
277224
// Is there a more direct way to see if RCS is enabled? module.isEnabled doesn't work...
278-
else if (module is ModuleRCS && vessel.ActionGroups[KSPActionGroup.RCS])
225+
else if (rcsModule != null && vessel.ActionGroups[KSPActionGroup.RCS])
279226
{
280-
ModuleRCS rcs = ((ModuleRCS)module);
281-
282-
foreach (Transform thruster in rcs.thrusterTransforms) {
283-
// Avoids problems with part.Rigidbody.centerOfMass; should also give better
227+
var vesselTransform = vessel.GetTransform();
228+
foreach (Transform thruster in rcsModule.thrusterTransforms)
229+
{
230+
// Avoids problems with part.Rigidbody.centerOfMass; should also give better
284231
// support for RCS units integrated into larger parts
285-
Vector3d thrusterOffset = vessel.GetTransform().InverseTransformPoint(
286-
thruster.position) - CoM;
232+
Vector3d thrusterOffset = vesselTransform.InverseTransformPoint(thruster.position) - centerOfMass;
287233
/* Code by sarbian, shamelessly copied from MechJeb */
288-
Vector3d thrusterThrust = vessel.GetTransform().InverseTransformDirection(
289-
-thruster.up.normalized) * rcs.thrusterPower;
234+
Vector3d thrusterThrust = vesselTransform.InverseTransformDirection(-thruster.up.normalized) * rcsModule.thrusterPower;
290235
Vector3d thrusterTorque = Vector3.Cross(thrusterOffset, thrusterThrust);
291236
/* end sarbian's code */
292237

293238
// This overestimates the usable torque, but that doesn't change the final behavior much
294239
pitch += (float)Math.Abs(thrusterTorque.x);
295-
roll += (float)Math.Abs(thrusterTorque.y);
296-
yaw += (float)Math.Abs(thrusterTorque.z);
240+
roll += (float)Math.Abs(thrusterTorque.y);
241+
yaw += (float)Math.Abs(thrusterTorque.z);
297242
}
298243
}
299244
}
300245

301-
float gimbal = (float)GetThrustTorque (part, vessel) * thrust;
246+
float gimbal = (float)GetThrustTorque(part, vessel) * thrust;
302247
pitch += gimbal;
303-
yaw += gimbal;
248+
yaw += gimbal;
304249
}
305250

306251
return new Vector3d(pitch, roll, yaw);
307252
}
308253

309254
public static double GetThrustTorque(Part p, Vessel vessel)
310255
{
311-
var CoM = vessel.CoM;
256+
var centerOfMass = vessel.CoM;
312257

313258
if (p.State == PartStates.ACTIVE)
314259
{
315260
if (p is LiquidEngine)
316261
{
317262
if (((LiquidEngine)p).thrustVectoringCapable)
318263
{
319-
return Math.Sin(Math.Abs(((LiquidEngine)p).gimbalRange) * Math.PI / 180) * ((LiquidEngine)p).maxThrust * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
264+
return Math.Sin(Math.Abs(((LiquidEngine)p).gimbalRange) * Math.PI / 180) * ((LiquidEngine)p).maxThrust * (p.Rigidbody.worldCenterOfMass - centerOfMass).magnitude;
320265
}
321266
}
322267
else if (p is LiquidFuelEngine)
323268
{
324269
if (((LiquidFuelEngine)p).thrustVectoringCapable)
325270
{
326-
return Math.Sin(Math.Abs(((LiquidFuelEngine)p).gimbalRange) * Math.PI / 180) * ((LiquidFuelEngine)p).maxThrust * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
271+
return Math.Sin(Math.Abs(((LiquidFuelEngine)p).gimbalRange) * Math.PI / 180) * ((LiquidFuelEngine)p).maxThrust * (p.Rigidbody.worldCenterOfMass - centerOfMass).magnitude;
327272
}
328273
}
329274
else if (p is AtmosphericEngine)
330275
{
331276
if (((AtmosphericEngine)p).thrustVectoringCapable)
332277
{
333-
return Math.Sin(Math.Abs(((AtmosphericEngine)p).gimbalRange) * Math.PI / 180) * ((AtmosphericEngine)p).maximumEnginePower * ((AtmosphericEngine)p).totalEfficiency * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
278+
return Math.Sin(Math.Abs(((AtmosphericEngine)p).gimbalRange) * Math.PI / 180) * ((AtmosphericEngine)p).maximumEnginePower * ((AtmosphericEngine)p).totalEfficiency * (p.Rigidbody.worldCenterOfMass - centerOfMass).magnitude;
334279
}
335280
}
336281
}
@@ -356,24 +301,5 @@ public static Vector3d Sign(Vector3d vector)
356301
{
357302
return new Vector3d(Math.Sign(vector.x), Math.Sign(vector.y), Math.Sign(vector.z));
358303
}
359-
360-
private static Vector3d averageVector3d(Vector3d[] vectorArray, Vector3d newVector, int n)
361-
{
362-
double x = 0.0, y = 0.0, z = 0.0;
363-
int k = 0;
364-
365-
// Loop through the array to determine average
366-
// Give more weight to newer items and less weight to older items
367-
for (int i = 0; i < n; i++)
368-
{
369-
k += i + 1;
370-
if (i < n - 1) { vectorArray[i] = vectorArray[i + 1]; }
371-
else { vectorArray[i] = newVector; }
372-
x += vectorArray[i].x * (i + 1);
373-
y += vectorArray[i].y * (i + 1);
374-
z += vectorArray[i].z * (i + 1);
375-
}
376-
return new Vector3d(x / k, y / k, z / k);
377-
}
378304
}
379305
}

0 commit comments

Comments
 (0)