public override void Drive(FlightCtrlState s)
{
Vector3d worldVelocityDelta = vesselState.velocityVesselOrbit - targetVelocity;
worldVelocityDelta += TimeWarp.fixedDeltaTime * vesselState.gravityForce; //account for one frame's worth of gravity
Vector3d velocityDelta = Quaternion.Inverse(vessel.GetTransform().rotation) * worldVelocityDelta;
if (!conserveFuel || (velocityDelta.magnitude > conserveThreshold))
{
if (!vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
}
Vector3d rcs = new Vector3d();
foreach (Vector6.Direction dir in Enum.GetValues(typeof(Vector6.Direction)))
{
if (vesselState.rcsThrustAvailable[dir] > 0)
{
double dV = Vector3d.Dot(velocityDelta, Vector6.directions[dir]) / (vesselState.rcsThrustAvailable[dir] * TimeWarp.fixedDeltaTime / vesselState.mass);
if (dV > 0)
{
rcs += Vector6.directions[dir] * dV;
}
}
}
//rcs = pid.Compute(rcs, rcs - lastAct); // Having an Omega would be nice but each test made it worse
rcs = pid.Compute(rcs);
//rcs = lastAct + (rcs - lastAct) * (1 / ((0.5 / TimeWarp.fixedDeltaTime) + 1));
lastAct = rcs;
s.X = Mathf.Clamp((float)rcs.x, -1, 1);
s.Y = Mathf.Clamp((float)rcs.z, -1, 1); //note that z and
s.Z = Mathf.Clamp((float)rcs.y, -1, 1); //y must be swapped
}
else if (conserveFuel)
{
if (vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, false);
}
}
base.Drive(s);
}
public override void Drive(FlightCtrlState s)
{
// Used in the killRot activation calculation and drive_limit calculation
double precision = Math.Max(0.5, Math.Min(10.0, (Math.Min(vesselState.torqueAvailable.x, vesselState.torqueAvailable.z) + vesselState.torqueThrustPYAvailable * s.mainThrottle) * 20.0 / vesselState.MoI.magnitude));
// Reset the PID controller during roll to keep pitch and yaw errors
// from accumulating on the wrong axis.
double rollDelta = Mathf.Abs((float)(vesselState.vesselRoll - lastResetRoll));
if (rollDelta > 180)
{
rollDelta = 360 - rollDelta;
}
if (rollDelta > 5)
{
pid.Reset();
lastResetRoll = vesselState.vesselRoll;
}
// Direction we want to be facing
Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * target);
Vector3d deltaEuler = new Vector3d(
(delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
-((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
(delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
);
Vector3d torque = new Vector3d(
vesselState.torqueAvailable.x + vesselState.torqueThrustPYAvailable * s.mainThrottle,
vesselState.torqueAvailable.y,
vesselState.torqueAvailable.z + vesselState.torqueThrustPYAvailable * s.mainThrottle
);
Vector3d inertia = Vector3d.Scale(
vesselState.angularMomentum.Sign(),
Vector3d.Scale(
Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum),
Vector3d.Scale(torque, vesselState.MoI).Invert()
)
);
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += inertia.Reorder(132);
err.Scale(Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132));
Vector3d act = pid.Compute(err);
float drive_limit = Mathf.Clamp01((float)(err.magnitude * drive_factor / precision));
act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit);
act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit);
act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit);
act = lastAct + (act - lastAct) * (TimeWarp.fixedDeltaTime / Tf);
SetFlightCtrlState(act, deltaEuler, s, precision, drive_limit);
act = new Vector3d(s.pitch, s.yaw, s.roll);
lastAct = act;
stress = Math.Abs(act.x) + Math.Abs(act.y) + Math.Abs(act.z);
}