virtual public void FixedUpdate()
{
if (fsm != null && fsm.Started)
{
fsm.FixedUpdateFSM();
}
}
/// <summary>
/// Internal flight control processing for the vessel.
/// </summary>
private void PilotFixedUpdate()
{
if (maneuverPilotEngaged)
{
//Utility.LogMessage(this, "FixedUpdate: FSM state is {0}, started = {1}", maneuverPilot.currentStateName, maneuverPilot.Started);
maneuverPilot.FixedUpdateFSM();
}
if (attitudePilotEngaged && vessel.ActionGroups[KSPActionGroup.SAS])
{
if (vessel.Autopilot.Mode == VesselAutopilot.AutopilotMode.StabilityAssist)
{
Quaternion goalOrientation = getReferenceAttitude(activeReference) * relativeOrientation;// *Quaternion.Euler(90, 0, 0);
Quaternion currentOrientation = vessel.Autopilot.SAS.lockedRotation;
float delta = Quaternion.Angle(goalOrientation, currentOrientation);
Quaternion newOrientation;
// Reduce the angle updates as the vessel gets closer to the target orientation.
if (delta < 0.25f)
{
newOrientation = goalOrientation;
}
else if (delta > 10.0f)
{
newOrientation = Quaternion.Slerp(currentOrientation, goalOrientation, 2.0f / delta);
}
else if (delta > 3.0f)
{
newOrientation = Quaternion.Slerp(currentOrientation, goalOrientation, 0.5f / delta);
}
else // 0.25 - 3 degrees difference
{
newOrientation = Quaternion.Slerp(currentOrientation, goalOrientation, 0.15f / delta);
}
vessel.Autopilot.SAS.LockRotation(newOrientation);
}
else
{
CancelAutopilots();
}
}
else
{
CancelAutopilots();
}
}