Esempio n. 1
0
        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();
            }
        }
Esempio n. 2
0
        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);
            }
        }