Skip to content

Commit 5878df6

Browse files
d4rksh4ded4rksh4de
committed
Revert "Fixed FlightComputer's attitude controller being unstable and spinning. The FlightComputer's attitude controller was based on an old version of MechJeb's attitude controller, integrated the latest MechJeb's changes."
This reverts commit 70d0904.
1 parent 70d0904 commit 5878df6

5 files changed

Lines changed: 191 additions & 237 deletions

File tree

‎src/RemoteTech/FlightComputer/FlightComputer.cs‎

Lines changed: 9 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ public State Status
7979
public AttitudeCommand currentFlightMode { get { return (mActiveCommands[0] is AttitudeCommand) ? (AttitudeCommand)mActiveCommands[0] : null; } }
8080

8181
// Flight controller parameters from MechJeb, copied from master on June 27, 2014
82-
public PIDControllerV3 pid { get; private set; }
82+
public PIDControllerV2 pid { get; private set; }
8383
public Vector3d lastAct { get; set; }
8484
public double Tf = 0.3;
8585
public double TfMin = 0.1;
@@ -102,8 +102,8 @@ public FlightComputer(ISignalProcessor s)
102102
SignalProcessor = s;
103103
Vessel = s.Vessel;
104104
SanctionedPilots = new List<Action<FlightCtrlState>>();
105-
pid = new PIDControllerV3(Vector3d.zero, Vector3d.zero, Vector3d.zero, 1, -1);
106-
setPIDParameters();
105+
pid = new PIDControllerV2(0, 0, 0, 1, -1);
106+
initPIDParameters();
107107
lastAct = Vector3d.zero;
108108
lastTarget = TargetCommand.WithTarget(null);
109109

@@ -357,19 +357,12 @@ private void OnFlyByWirePost(FlightCtrlState fcs)
357357
}
358358
}
359359

360-
public void setPIDParameters()
360+
public void initPIDParameters()
361361
{
362-
Vector3d TfV = new Vector3d(0.3, 0.3, 0.3);
363-
Vector3d invTf = TfV.Invert();
364-
pid.Kd = kdFactor * invTf;
365-
366-
pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * pid.Kd;
367-
pid.Kp.Scale(invTf);
368-
369-
pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * pid.Kp;
370-
pid.Ki.Scale(invTf);
371-
372-
pid.intAccum = pid.intAccum.Clamp(-5, 5);
362+
pid.Kd = kdFactor / Tf;
363+
pid.Kp = pid.Kd / (kpFactor * Math.Sqrt(2) * Tf);
364+
pid.Ki = pid.Kp / (kiFactor * Math.Sqrt(2) * Tf);
365+
pid.intAccum = Vector3.ClampMagnitude(pid.intAccum, 5);
373366
}
374367

375368
// Calculations of Tf are not safe during FlightComputer constructor
@@ -392,7 +385,7 @@ public void updatePIDParameters()
392385
Tf = Mathf.Clamp((float)ratio.magnitude / 20f, 2 * TimeWarp.fixedDeltaTime, 1f);
393386
Tf = Mathf.Clamp((float)Tf, (float)TfMin, (float)TfMax);
394387
}
395-
setPIDParameters();
388+
initPIDParameters();
396389
}
397390

398391
/// <summary>

‎src/RemoteTech/FlightComputer/FlightCore.cs‎

Lines changed: 104 additions & 84 deletions
Original file line numberDiff line numberDiff line change
@@ -176,110 +176,58 @@ public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightC
176176
// Add support for roll-less targets later -- Starstrider42
177177
bool fixedRoll = !ignoreRoll;
178178
Vessel vessel = fc.Vessel;
179-
Vector3d momentOfInertia = vessel.MOI;
179+
Vector3d momentOfInertia = GetTrueMoI(vessel);
180180
Transform vesselReference = vessel.GetTransform();
181-
Vector3d torque = GetTorque(vessel, c.mainThrottle);
182-
183-
// -----------------------------------------------
184-
// Copied from MechJeb master on 18.04.2016 with some modifications to adapt to RemoteTech
185-
186-
Vector3d _axisControl = new Vector3d();
187-
_axisControl.x = true ? 1 : 0;
188-
_axisControl.y = true ? 1 : 0;
189-
_axisControl.z = fixedRoll ? 1 : 0;
190-
191-
Vector3d inertia = Vector3d.Scale(
192-
new Vector3d(vessel.angularMomentum.x, vessel.angularMomentum.y, vessel.angularMomentum.z).Sign(),
193-
Vector3d.Scale(
194-
Vector3d.Scale(vessel.angularMomentum, vessel.angularMomentum),
195-
Vector3d.Scale(torque, momentOfInertia).Invert()
196-
)
197-
);
198181

199-
Vector3d TfV = new Vector3d(0.3, 0.3, 0.3);
200-
201-
double kpFactor = 3;
202-
double kiFactor = 6;
203-
double kdFactor = 0.5;
204-
double kWlimit = 0.15;
205-
double deadband = 0.0001;
182+
//---------------------------------------
183+
// Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42
206184

207185
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);
208186

209-
Vector3d deltaEuler = delta.DeltaEuler();
187+
Vector3d torque = GetTorque(vessel, c.mainThrottle);
188+
Vector3d spinMargin = GetStoppingAngle(vessel, torque);
210189

211-
// ( MoI / available torque ) factor:
212-
Vector3d NormFactor = Vector3d.Scale(momentOfInertia, torque.Invert()).Reorder(132);
190+
// Allow for zero torque around some but not all axes
191+
Vector3d normFactor;
192+
normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0);
193+
normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0);
194+
normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0);
195+
normFactor = SwapYZ(normFactor);
213196

214-
// Find out the real shorter way to turn were we wan to.
197+
// Find out the real shorter way to turn were we want to.
215198
// Thanks to HoneyFox
216-
Vector3d tgtLocalUp = vesselReference.rotation.Inverse() * target * Vector3d.forward;
199+
200+
Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward;
217201
Vector3d curLocalUp = Vector3d.up;
218202

219203
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
220-
Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
221-
rotDirection = rotDirection.normalized * turnAngle / 180.0;
222-
223-
// And the lowest roll
224-
// Thanks to Crzyrndm
225-
Vector3 normVec = Vector3.Cross(target * Vector3.forward, vesselReference.up);
226-
Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * target;
227-
float rollError = Vector3.Angle(vesselReference.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselReference.forward));
204+
var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
205+
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
228206

229-
var error = new Vector3d(
207+
var err = new Vector3d(
230208
-rotDirection.y * Math.PI,
231209
rotDirection.x * Math.PI,
232-
rollError * Mathf.Deg2Rad
233-
);
234-
235-
error.Scale(_axisControl);
236-
237-
Vector3d err = error + inertia.Reorder(132) / 2d;
238-
err = new Vector3d(
239-
Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
210+
fixedRoll ?
211+
((delta.eulerAngles.z > 180) ?
212+
(delta.eulerAngles.z - 360.0F) :
213+
delta.eulerAngles.z) * Math.PI / 180.0F
214+
: 0F
215+
);
216+
217+
err += SwapYZ(spinMargin);
218+
err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
240219
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
241220
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
242-
243-
err.Scale(NormFactor);
221+
err.Scale(normFactor);
244222

245223
// angular velocity:
246-
Vector3d omega;
247-
omega.x = vessel.angularVelocity.x;
248-
omega.y = vessel.angularVelocity.z; // y <=> z
249-
omega.z = vessel.angularVelocity.y; // z <=> y
250-
omega.Scale(NormFactor);
251-
252-
//if (Tf_autoTune)
253-
// tuneTf(torque);
224+
Vector3d omega = SwapYZ(vessel.GetComponent<Rigidbody>().angularVelocity);
225+
omega.Scale(normFactor);
254226

255-
Vector3d invTf = TfV.Invert();
256-
fc.pid.Kd = kdFactor * invTf;
227+
Vector3d pidAction = fc.pid.Compute(err, omega);
257228

258-
fc.pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * fc.pid.Kd;
259-
fc.pid.Kp.Scale(invTf);
260-
261-
fc.pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * fc.pid.Kp;
262-
fc.pid.Ki.Scale(invTf);
263-
264-
fc.pid.intAccum = fc.pid.intAccum.Clamp(-5, 5);
265-
266-
// angular velocity limit:
267-
var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit),
268-
Math.Sqrt(NormFactor.y * Math.PI * kWlimit),
269-
Math.Sqrt(NormFactor.z * Math.PI * kWlimit));
270-
271-
Vector3d pidAction = fc.pid.Compute(err, omega, Wlimit);
272-
273-
// deadband
274-
pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0;
275-
pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0;
276-
pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0;
277-
278-
// low pass filter, wf = 1/Tf:
279-
Vector3d act = fc.lastAct;
280-
act.x += (pidAction.x - fc.lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0));
281-
act.y += (pidAction.y - fc.lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0));
282-
act.z += (pidAction.z - fc.lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0));
229+
// low pass filter, wf = 1/Tf:
230+
Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1));
283231
fc.lastAct = act;
284232

285233
// end MechJeb import
@@ -333,6 +281,78 @@ private class Matrix3x3
333281
}
334282
}
335283

284+
/// <summary>
285+
/// Returns a more accurate moment of inertia than Vessel.findLocalMOI()
286+
/// </summary>
287+
// Copied from MechJeb master on June 27, 2014
288+
// TODO: cache moment if inertia and update only when ship mass changes?
289+
private static Vector3d GetTrueMoI(Vessel vessel)
290+
{
291+
var inertiaTensor = new Matrix3x3();
292+
var centerOfMass = vessel.findWorldCenterOfMass();
293+
294+
foreach (Part p in vessel.parts)
295+
{
296+
if (p.Rigidbody == null) continue;
297+
298+
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
299+
Vector3d principalMoments = p.Rigidbody.inertiaTensor;
300+
Quaternion princAxesRot = Quaternion.Inverse(vessel.GetTransform().rotation) * p.transform.rotation * p.Rigidbody.inertiaTensorRotation;
301+
Quaternion invPrincAxesRot = Quaternion.Inverse(princAxesRot);
302+
303+
for (int i = 0; i < 3; i++)
304+
{
305+
Vector3d iHat = Vector3d.zero;
306+
iHat[i] = 1;
307+
for (int j = 0; j < 3; j++)
308+
{
309+
Vector3d jHat = Vector3d.zero;
310+
jHat[j] = 1;
311+
inertiaTensor[i, j] += Vector3d.Dot(iHat, princAxesRot * Vector3d.Scale(principalMoments, invPrincAxesRot * jHat));
312+
}
313+
}
314+
315+
//Compute the contributions to the vessel inertia tensor due to the part mass and position
316+
double partMass = p.mass + p.GetResourceMass();
317+
Vector3 partPosition = vessel.GetTransform().InverseTransformDirection(p.Rigidbody.worldCenterOfMass - centerOfMass);
318+
319+
for (int i = 0; i < 3; i++)
320+
{
321+
inertiaTensor[i, i] += partMass * partPosition.sqrMagnitude;
322+
323+
for (int j = 0; j < 3; j++)
324+
{
325+
inertiaTensor[i, j] += -partMass * partPosition[i] * partPosition[j];
326+
}
327+
}
328+
}
329+
330+
return new Vector3d(inertiaTensor[0, 0], inertiaTensor[1, 1], inertiaTensor[2, 2]);
331+
}
332+
333+
/// <summary>
334+
/// Calculates how far a ship can rotate before its rotation stops.
335+
/// </summary>
336+
/// <returns>A vector equal to the stopping angle, in radians, around the (pitch, roll, yaw) axes.
337+
/// If it is impossible to stop the ship along one axis, returns 0 for that axis.</returns>
338+
/// <param name="vessel">The ship whose rotation needs to be stopped.</param>
339+
/// <param name="torque">The torque that can be applied to the ship.</param>
340+
public static Vector3d GetStoppingAngle(Vessel vessel, Vector3d torque)
341+
{
342+
var momentOfInertia = GetTrueMoI(vessel);
343+
var angularVelocity = Quaternion.Inverse(vessel.transform.rotation) * vessel.GetComponent<Rigidbody>().angularVelocity;
344+
var angularMomentum = Vector3d.Scale(angularVelocity, momentOfInertia);
345+
346+
// Adapted from MechJeb master on June 27, 2014
347+
Vector3d retVar;
348+
retVar.x = (torque.x != 0.0 ? angularMomentum.x * angularMomentum.x / (torque.x * momentOfInertia.x) : 0.0);
349+
retVar.y = (torque.y != 0.0 ? angularMomentum.y * angularMomentum.y / (torque.y * momentOfInertia.y) : 0.0);
350+
retVar.z = (torque.z != 0.0 ? angularMomentum.z * angularMomentum.z / (torque.z * momentOfInertia.z) : 0.0);
351+
retVar.Scale(Sign(angularMomentum));
352+
353+
return retVar / 2;
354+
}
355+
336356
/// <summary>
337357
/// Returns the torque the ship can exert around its center of mass
338358
/// </summary>

‎src/RemoteTech/FlightComputer/PIDController.cs‎

Lines changed: 0 additions & 85 deletions
This file was deleted.

0 commit comments

Comments
 (0)