private void SurfaceSAS(FlightCtrlState state)
{
if (bArmed)
{
FlightData.updateAttitude();
pauseManager(state);
float vertResponse = 0;
if (bActive[(int)SASList.Pitch])
{
vertResponse = -1 * (float)Utils.GetSAS(SASList.Pitch).ResponseD(FlightData.pitch);
}
float hrztResponse = 0;
if (bActive[(int)SASList.Yaw] && (FlightData.thisVessel.latitude <88 && FlightData.thisVessel.latitude> -88))
{
if (Utils.GetSAS(SASList.Yaw).SetPoint - FlightData.heading >= -180 && Utils.GetSAS(SASList.Yaw).SetPoint - FlightData.heading <= 180)
{
hrztResponse = -1 * (float)Utils.GetSAS(SASList.Yaw).ResponseD(FlightData.heading);
}
else if (Utils.GetSAS(SASList.Yaw).SetPoint - FlightData.heading < -180)
{
hrztResponse = -1 * (float)Utils.GetSAS(SASList.Yaw).ResponseD(FlightData.heading - 360);
}
else if (Utils.GetSAS(SASList.Yaw).SetPoint - FlightData.heading > 180)
{
hrztResponse = -1 * (float)Utils.GetSAS(SASList.Yaw).ResponseD(FlightData.heading + 360);
}
}
else
{
Utils.GetSAS(SASList.Yaw).SetPoint = FlightData.heading;
}
double rollRad = Math.PI / 180 * FlightData.roll;
if ((!bPause[(int)SASList.Pitch] || !bPause[(int)SASList.Yaw]) && (bActive[(int)SASList.Pitch] || bActive[(int)SASList.Yaw]))
{
state.pitch = (vertResponse * (float)Math.Cos(rollRad) - hrztResponse * (float)Math.Sin(rollRad)) / activationFadePitch;
state.yaw = (vertResponse * (float)Math.Sin(rollRad) + hrztResponse * (float)Math.Cos(rollRad)) / activationFadeYaw;
}
rollResponse();
}
}
private void vesselController(FlightCtrlState state)
{
if (!HighLogic.LoadedSceneIsFlight)
{
return;
}
if (FlightData.thisVessel == null)
{
return;
}
FlightData.updateAttitude();
if (IsPaused())
{
return;
}
// Heading Control
if (bHdgActive)
{
if (!bWingLeveller && (FlightData.thisVessel.latitude < 88 && FlightData.thisVessel.latitude > -88))
{
if (Utils.GetAsst(PIDList.HdgBank).SetPoint - FlightData.heading >= -180 && Utils.GetAsst(PIDList.HdgBank).SetPoint - FlightData.heading <= 180)
{
Utils.GetAsst(PIDList.Aileron).SetPoint = Utils.GetAsst(PIDList.HdgBank).ResponseD(FlightData.heading);
Utils.GetAsst(PIDList.HdgYaw).SetPoint = Utils.GetAsst(PIDList.HdgBank).ResponseD(FlightData.heading);
}
else if (Utils.GetAsst(PIDList.HdgBank).SetPoint - FlightData.heading < -180)
{
Utils.GetAsst(PIDList.Aileron).SetPoint = Utils.GetAsst(PIDList.HdgBank).ResponseD(FlightData.heading - 360);
Utils.GetAsst(PIDList.HdgYaw).SetPoint = Utils.GetAsst(PIDList.HdgBank).ResponseD(FlightData.heading - 360);
}
else if (Utils.GetAsst(PIDList.HdgBank).SetPoint - FlightData.heading > 180)
{
Utils.GetAsst(PIDList.Aileron).SetPoint = Utils.GetAsst(PIDList.HdgBank).ResponseD(FlightData.heading + 360);
Utils.GetAsst(PIDList.HdgYaw).SetPoint = Utils.GetAsst(PIDList.HdgBank).ResponseD(FlightData.heading + 360);
}
Utils.GetAsst(PIDList.Rudder).SetPoint = -Utils.GetAsst(PIDList.HdgYaw).ResponseD(FlightData.yaw);
}
else
{
bWasWingLeveller = true;
Utils.GetAsst(PIDList.Aileron).SetPoint = 0;
Utils.GetAsst(PIDList.Rudder).SetPoint = 0;
}
state.roll = Utils.GetAsst(PIDList.Aileron).ResponseF(FlightData.roll);
state.yaw = Utils.GetAsst(PIDList.Rudder).ResponseF(FlightData.yaw);
}
if (bVertActive)
{
// Set requested vertical speed
if (bAltitudeHold)
{
Utils.GetAsst(PIDList.VertSpeed).SetPoint = -Utils.GetAsst(PIDList.Altitude).ResponseD(FlightData.thisVessel.altitude);
}
Utils.GetAsst(PIDList.Elevator).SetPoint = -Utils.GetAsst(PIDList.VertSpeed).ResponseD(FlightData.thisVessel.verticalSpeed);
state.pitch = -Utils.GetAsst(PIDList.Elevator).ResponseF(FlightData.AoA);
}
if (bThrottleActive)
{
state.mainThrottle = -Utils.GetAsst(PIDList.Throttle).ResponseF(FlightData.thisVessel.srfSpeed);
}
}