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); } }