C# FlightCtrlState类代码示例

            public override AutopilotStep Drive(FlightCtrlState s)
            {
                if (!core.landing.PredictionReady)
                    return this;

                Vector3d deltaV;
                try
                {
                    deltaV = core.landing.ComputeCourseCorrection(true);
                }
                catch(ArgumentException e)
                {
                    status = e.Message;
                    return new DecelerationBurn(core);
                }

                if (core.landing.rcsAdjustment)
                {
                    if (deltaV.magnitude > 3)
                        core.rcs.enabled = true;
                    else if (deltaV.magnitude < 0.01)
                        core.rcs.enabled = false;

                    if (core.rcs.enabled)
                        core.rcs.SetWorldVelocityError(deltaV);
                }

                return this;
            }

        public void DriveAutoland(FlightCtrlState s)
        {
            if (!part.vessel.Landed)
            {
                Vector3d runwayStart = RunwayStart();

                if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0)
                {
                    vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true);
                    loweredGear = true;
                }

                Vector3d vectorToWaypoint = ILSAimDirection();
                double headingToWaypoint = vesselState.HeadingFromDirection(vectorToWaypoint);

                Vector3d vectorToRunway = runwayStart - vesselState.CoM;
                double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up) + (vesselState.altitudeTrue - vesselState.altitudeBottom);
                double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway);
                double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway);
                double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 0.0F);

                AimVelocityVector(desiredFPA, headingToWaypoint);
            }
            else
            {
                //keep the plane aligned with the runway:
                Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM);
                if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1;
                double runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north));
                core.attitude.attitudeTo(runwayHeading, 0, 0, this);
            }
        }

        protected override void _Drive(FlightCtrlState s)
        {
            Vessel vessel = base.Autopilot.vessel;
            AttitudeController attitudeController = base.Autopilot.AutopilotController.AttitudeController;

            if (vessel.altitude < 10000) {
                attitudeController.Attitude = Attitude.Up;
                base.Autopilot.AutopilotController.AutoStagingController.Enable ();
                s.mainThrottle = 1.0f;
            } else if (vessel.altitude < 80000) {
                attitudeController.Pitch = 90;
                attitudeController.Yaw = 45;
                attitudeController.Attitude = Attitude.UserDefined;
                s.mainThrottle = Mathf.Clamp01 (100000.0f - (float)vessel.orbit.ApA);
            } else {
                double ut = Planetarium.GetUniversalTime () + vessel.orbit.timeToAp;

                double deltaV = Math.Sqrt (vessel.mainBody.gravParameter / (100000 + vessel.mainBody.Radius)) - vessel.orbit.getOrbitalVelocityAtUT (ut).magnitude;

                Vector3d nodeDV = new Vector3d (0, 0, deltaV);
                base.Autopilot.ManeuverNode = vessel.patchedConicSolver.AddManeuverNode (ut);
                base.Autopilot.ManeuverNode.OnGizmoUpdated (nodeDV, ut);

                base.Autopilot.AutopilotController.Disable ();
                base.Autopilot.AutopilotController.AutoStagingController.Disable ();
                attitudeController.Attitude = Attitude.None;
            }
        }

 public void OnFlyByWire(FlightCtrlState c)
 {
     foreach (LockableControl control in controls)
     {
         control.OnFlyByWire(ref c);
     }
 }

        /// <summary>
        /// If the mission is on a client controlled mission, we disable every incoming input
        /// </summary>
        /// <param name="s">S.</param>
        private void onFlyByWire(FlightCtrlState s)
        {
            Status status = calculateStatus(currentMission, false, activeVessel);

            if(status.isClientControlled) {
                s.fastThrottle = 0;
                s.gearDown = false;
                s.gearUp = false;
                s.headlight = false;
                s.killRot = false;
                s.mainThrottle = 0;
                s.pitch = 0;
                s.pitchTrim = 0;
                s.roll = 0;
                s.rollTrim = 0;
                s.X = 0;
                s.Y = 0;
                s.yaw = 0;
                s.yawTrim = 0;
                s.Z = 0;
                s.wheelSteer = 0;
                s.wheelSteerTrim = 0;
                s.wheelThrottle = 0;
                s.wheelThrottleTrim = 0;
                s.NeutralizeAll ();
            }
        }

        public override bool Execute(FlightComputer f, FlightCtrlState fcs)
        {
            if (mAbort)
            {
                fcs.mainThrottle = 0.0f;
                return true;
            }

            if (Duration > 0)
            {
                fcs.mainThrottle = Throttle;
                Duration -= TimeWarp.deltaTime;
            }
            else if (DeltaV > 0)
            {
                fcs.mainThrottle = Throttle;
                DeltaV -= (Throttle * FlightCore.GetTotalThrust(f.Vessel) / f.Vessel.GetTotalMass()) * TimeWarp.deltaTime;
            }
            else
            {
                fcs.mainThrottle = 0.0f;
                return true;
            }
            return false;
        }

 public void postAutoPilotUpdate(FlightCtrlState state)
 {
     if (vesselRef.HoldPhysics)
         return;
     vesselSSAS.SurfaceSAS(state);
     vesselAsst.vesselController(state);
 }

        public override void drive(FlightCtrlState s)
        {
            if (autoLand)
            {
                if (!part.vessel.Landed)
                {
                    if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0)
                    {
                        part.vessel.rootPart.SendEvent("LowerLandingGear");
                        loweredGear = true;
                    }

                    Vector3d vectorToWaypoint = waypoint.transform.position - vesselState.CoM;
                    double headingToWaypoint = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vectorToWaypoint, vesselState.east), Vector3d.Dot(vectorToWaypoint, vesselState.north));

                    double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 0.0F);

                    aimVelocityVector(desiredFPA, headingToWaypoint);
                }
                else
                {
                    //keep the plane aligned with the runway:
                    Vector3d target = runwayEnd;
                    if (Vector3d.Dot(target - vesselState.CoM, vesselState.forward) < 0) target = runwayStart;
                    core.attitudeTo((target- vesselState.CoM).normalized, MechJebCore.AttitudeReference.INERTIAL, this);
                }
            }
            else if (holdHeadingAndAlt)
            {
                double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / 30.0;
                double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9)));
                aimVelocityVector(targetFlightPathAngle, targetHeading);
            }
        }

 public void OnFlyByWire(FlightCtrlState c)
 {
     foreach (var control in controls)
     {
         control.OnFlyByWire(ref c);
     }
 }

        public void ButtonPressedCallback(IController controller, int button, FlightCtrlState state)
        {
            if(!HighLogic.LoadedSceneIsFlight)
            {
                return;
            }

            var config = m_Configuration.GetConfigurationByIController(controller);

            Bitset mask = controller.GetButtonsMask();

            if (config.evaluatedDiscreteActionMasks.Contains(mask))
            {
                return;
            }

            List<DiscreteAction> actions = config.GetCurrentPreset().GetDiscreteBinding(mask);

            if(actions != null)
            {
                foreach (DiscreteAction action in actions)
                {
                    m_FlightManager.EvaluateDiscreteAction(config, action, state);
                    config.evaluatedDiscreteActionMasks.Add(mask);
                }
            }
        }

		void SetAcceleration(float accel, FlightCtrlState s)
		{
			float gravAccel = GravAccel();
			float requestEngineAccel = accel - gravAccel;

			possibleAccel = gravAccel;

			float dragAccel = 0;
			float engineAccel = MaxEngineAccel(requestEngineAccel, out dragAccel);

			if(engineAccel == 0)
			{
				s.mainThrottle = 0;
				return;
			}

			requestEngineAccel = Mathf.Clamp(requestEngineAccel, -engineAccel, engineAccel);

			float requestThrottle = (requestEngineAccel - dragAccel) / engineAccel;

			s.mainThrottle = Mathf.Clamp01(requestThrottle);

			//use brakes if overspeeding too much
			if(requestThrottle < -0.5f)
			{
				vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
			}
			else
			{
				vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
			}
		}

        public override void Drive(FlightCtrlState s)
        {
            if (controlHeading)
            {
                if (heading != headingLast)
                {
                    headingPID.Reset();
                    headingLast = heading;
                }

                double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
                headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
                if (s.wheelSteer == s.wheelSteerTrim)
                {
                    double act = headingPID.Compute(headingErr);
                    s.wheelSteer = Mathf.Clamp((float)act, -1, 1);
                }
            }
            if (controlSpeed)
            {
                if (speed != speedLast)
                {
                    speedPID.Reset();
                    speedLast = speed;
                }

                speedErr = speed - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.forward);
                if (s.wheelThrottle == s.wheelThrottleTrim)
                {
                    double act = speedPID.Compute(speedErr);
                    s.wheelThrottle = Mathf.Clamp((float)act, -1, 1);
                }
            }
        }

        public void HoldOrientation(FlightCtrlState s, Vessel v, Quaternion orientation, bool roll)
        {
            mVesselState.Update(v);
            // Used in the killRot activation calculation and drive_limit calculation
            double precision = Math.Max(0.5, Math.Min(10.0, (Math.Min(mVesselState.torqueAvailable.x, mVesselState.torqueAvailable.z) + mVesselState.torqueThrustPYAvailable * s.mainThrottle) * 20.0 / mVesselState.MoI.magnitude));

            // Reset the PID controller during roll to keep pitch and yaw errors
            // from accumulating on the wrong axis.
            double rollDelta = Mathf.Abs((float)(mVesselState.vesselRoll - lastResetRoll));
            if (rollDelta > 180)
                rollDelta = 360 - rollDelta;
            if (rollDelta > 5) {
                mPid.Reset();
                lastResetRoll = mVesselState.vesselRoll;
            }

            // Direction we want to be facing
            Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(v.ReferenceTransform.rotation) * orientation);

            Vector3d deltaEuler = new Vector3d(
                                                    (delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
                                                    -((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
                                                    (delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
                                                );

            Vector3d torque = new Vector3d(
                                                    mVesselState.torqueAvailable.x + mVesselState.torqueThrustPYAvailable * s.mainThrottle,
                                                    mVesselState.torqueAvailable.y,
                                                    mVesselState.torqueAvailable.z + mVesselState.torqueThrustPYAvailable * s.mainThrottle
                                            );

            Vector3d inertia = Vector3d.Scale(
                                                    mVesselState.angularMomentum.Sign(),
                                                    Vector3d.Scale(
                                                        Vector3d.Scale(mVesselState.angularMomentum, mVesselState.angularMomentum),
                                                        Vector3d.Scale(torque, mVesselState.MoI).Invert()
                                                    )
                                                );
            Vector3d err = deltaEuler * Math.PI / 180.0F;
            err += inertia.Reorder(132);
            err.Scale(Vector3d.Scale(mVesselState.MoI, torque.Invert()).Reorder(132));

            Vector3d act = mPid.Compute(err);

            float drive_limit = Mathf.Clamp01((float)(err.magnitude * drive_factor / precision));

            act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit);
            act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit);
            act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit);

            act = lastAct + (act - lastAct) * (TimeWarp.fixedDeltaTime / Tf);

            SetFlightCtrlState(act, deltaEuler, s, v, precision, drive_limit);

            act = new Vector3d(s.pitch, s.yaw, s.roll);
            lastAct = act;

            stress = Math.Abs(act.x) + Math.Abs(act.y) + Math.Abs(act.z);
        }

            public override AutopilotStep Drive(FlightCtrlState s)
            {
                if (!core.landing.PredictionReady)
                    return this;

                // If the atomospheric drag is at least 100mm/s2 then start trying to target the overshoot using the parachutes
                if (core.landing.deployChutes)
                {
                    if (core.landing.ParachutesDeployable())
                    {
                        core.landing.ControlParachutes();
                    }
                }

                double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), core.landing.LandingSite);

                if (currentError < 150)
                {
                    core.thrust.targetThrottle = 0;
                    core.rcs.enabled = core.landing.rcsAdjustment;
                    return new CoastToDeceleration(core);
                }

                // If a parachute has already been deployed then we will not be able to control attitude anyway, so move back to the coast to deceleration step.
                if (vesselState.parachuteDeployed)
                {
                    core.thrust.targetThrottle = 0;
                    return new CoastToDeceleration(core);
                }

                // We are not in .90 anymore. Turning while under drag is a bad idea
                if (vesselState.drag > 0.1)
                {
                    return new CoastToDeceleration(core);
                }

                Vector3d deltaV = core.landing.ComputeCourseCorrection(true);

                status = "Performing course correction of about " + deltaV.magnitude.ToString("F1") + " m/s";

                core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, core.landing);

                if (core.attitude.attitudeAngleFromTarget() < 2)
                    courseCorrectionBurning = true;
                else if (core.attitude.attitudeAngleFromTarget() > 30)
                    courseCorrectionBurning = false;

                if (courseCorrectionBurning)
                {
                    const double timeConstant = 2.0;
                    core.thrust.ThrustForDV(deltaV.magnitude, timeConstant);
                }
                else
                {
                    core.thrust.targetThrottle = 0;
                }

                return this;
            }

        public override void Drive(FlightCtrlState s)
        {
            setPIDParameters();

            // Removed the gravity since it also affect the target and we don't know the target pos here.
            // Since the difference is negligable for docking it's removed
            // TODO : add it back once we use the RCS Controler for other use than docking
            Vector3d worldVelocityDelta = vessel.obt_velocity - targetVelocity;
            //worldVelocityDelta += TimeWarp.fixedDeltaTime * vesselState.gravityForce; //account for one frame's worth of gravity
            //worldVelocityDelta -= TimeWarp.fixedDeltaTime * gravityForce = FlightGlobals.getGeeForceAtPosition(  Here be the target position  ); ; //account for one frame's worth of gravity

            // We work in local vessel coordinate
            Vector3d velocityDelta = Quaternion.Inverse(vessel.GetTransform().rotation) * worldVelocityDelta;

            if (!conserveFuel || (velocityDelta.magnitude > conserveThreshold))
            {
                if (!vessel.ActionGroups[KSPActionGroup.RCS])
                {
                    vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
                }

                Vector3d rcs = new Vector3d();

                foreach (Vector6.Direction dir in Enum.GetValues(typeof(Vector6.Direction)))
                {
                    double dirDv = Vector3d.Dot(velocityDelta, Vector6.directions[dir]);
                    double dirAvail = vesselState.rcsThrustAvailable[dir];
                    if (dirAvail  > 0 && Math.Abs(dirDv) > 0.001)
                    {
                        double dirAction = dirDv / (dirAvail * TimeWarp.fixedDeltaTime / vesselState.mass);
                        if (dirAction > 0)
                        {
                            rcs += Vector6.directions[dir] * dirAction;
                        }
                    }
                }

                Vector3d omega = Quaternion.Inverse(vessel.GetTransform().rotation) * (vessel.acceleration - vesselState.gravityForce);

                rcs = pid.Compute(rcs, omega);

                // Disabled the low pass filter for now. Was doing more harm than good
                //rcs = lastAct + (rcs - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
                lastAct = rcs;

                s.X = Mathf.Clamp((float)rcs.x, -1, 1);
                s.Y = Mathf.Clamp((float)rcs.z, -1, 1); //note that z and
                s.Z = Mathf.Clamp((float)rcs.y, -1, 1); //y must be swapped
            }
            else if (conserveFuel)
            {
                if (vessel.ActionGroups[KSPActionGroup.RCS])
                {
                    vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, false);
                }
            }

            base.Drive(s);
        }