//can probably be replaced with Vector3d.xzy?
public static Vector3d SwapYZ(Vector3d v)
{
return v.Reorder(132);
}
public override void Drive(FlightCtrlState s)
{
if (useSAS)
{
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
if (!part.vessel.ActionGroups[KSPActionGroup.SAS])
{
part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
lastSAS = _requestedAttitude;
}
else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10)
{
part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
lastSAS = _requestedAttitude;
}
else
{
part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude, true);
}
core.thrust.differentialThrottleDemandedTorque = Vector3d.zero;
}
else
{
// Direction we want to be facing
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Transform vesselTransform = vessel.ReferenceTransform;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselTransform.rotation) * _requestedAttitude);
Vector3d deltaEuler = delta.DeltaEuler();
torque = vesselState.torqueAvailable + vesselState.torqueFromEngine * vessel.ctrlState.mainThrottle;
if (core.thrust.differentialThrottleSuccess)
torque += vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle / 2.0;
inertia = Vector3d.Scale(
vesselState.angularMomentum.Sign(),
Vector3d.Scale(
Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum),
Vector3d.Scale(torque, vesselState.MoI).Invert()
)
);
// ( MoI / available torque ) factor:
Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);
// Find out the real shorter way to turn were we wan to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0;
// And the lowest roll
// Thanks to Crzyrndm
Vector3 normVec = Vector3.Cross(_requestedAttitude * Vector3.forward, vesselTransform.up);
Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * _requestedAttitude;
float rollError = Vector3.Angle(vesselTransform.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselTransform.forward));
error = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
rollError * Mathf.Deg2Rad
);
error.Scale(_axisControl);
Vector3d err = error + inertia.Reorder(132) / 2d;
err = new Vector3d(
Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(NormFactor);
// angular velocity:
Vector3d omega;
omega.x = vessel.angularVelocity.x;
omega.y = vessel.angularVelocity.z; // y <=> z
omega.z = vessel.angularVelocity.y; // z <=> y
omega.Scale(NormFactor);
if (Tf_autoTune)
tuneTf(torque);
setPIDParameters();
pidAction = pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = lastAct;
act.x += (pidAction.x - lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0));
act.y += (pidAction.y - lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0));
act.z += (pidAction.z - lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0));
lastAct = act;
SetFlightCtrlState(act, deltaEuler, s, 1);
act = new Vector3d(s.pitch, s.yaw, s.roll);
// Feed the control torque to the differential throttle
if (core.thrust.differentialThrottleSuccess)
core.thrust.differentialThrottleDemandedTorque = -Vector3d.Scale(act.xzy, vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle);
}
}