Ejemplo n.º 1
0
        void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
        {
            Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;

            float terrainAltitude;
            float dynPressure;
            float velocity;

            //Setup
            terrainAltitude = (float)vessel.heightFromTerrain;
            dynPressure     = (float)vessel.dynamicPressurekPa;
            velocity        = (float)vessel.srfSpeed;

            float upWeighting = pilot.UpWeighting(terrainAltitude, velocity);

            //Calculating errors
            ErrorData behavior = flightMode.Simulate(vesselTransform, velocityTransform, targetPosition, upDirection, upWeighting, vessel);

            //Controlling
            Steer steer = pilot.Simulate(behavior.pitchError, behavior.rollError, behavior.yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);

            //Piloting
            s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
            if (s.roll == s.rollTrim)
            {
                s.roll = Mathf.Clamp(steer.roll, -1, 1);
            }
            s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
        }
Ejemplo n.º 2
0
        public Steer Simulate(float pitchError, float rollError, float yawError, UnityEngine.Vector3 angVel, float terrainAltitude, float timestep, float dynPress, float vel)
        {
            float speedFactor = vel / dynPress / 16; //More work needs to be done to sanitize speedFactor

            if (speedFactor > 1.2f)
            {
                speedFactor = 1.2f;
            }

            float trimFactor = (float)Math.Sqrt(speedFactor);

            //AdaptGains(pitchError, rollError, yawError, angVel, terrainAltitude, timestep, dynPress, vel, trimFactor);

            float steerPitch = pitchPID.Simulate(pitchError, angVel.x, pIntLimit * trimFactor, timestep, speedFactor);
            float steerRoll  = rollPID.Simulate(rollError, angVel.y, rIntLimit, timestep, speedFactor);

            if (pitchPID.IntegralZeroed)        //yaw integrals should be zeroed at the same time that pitch PIDs are zeroed, because that happens in large turns
            {
                yawPID.ZeroIntegral();
            }
            float steerYaw = yawPID.Simulate(yawError, angVel.z, yIntLimit, timestep, speedFactor);

            Steer steer = new Steer(steerPitch, steerRoll, steerYaw);

            return(steer);
        }
        void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
        {
            Vector3d srfVel = vessel.srf_velocity;

            if (srfVel != Vector3d.zero)
            {
                velocityTransform.rotation = Quaternion.LookRotation(srfVel, -vesselTransform.forward);
            }
            velocityTransform.rotation = Quaternion.AngleAxis(90, velocityTransform.right) * velocityTransform.rotation;
            Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;

            Vector3d targetDirection;
            Vector3d targetDirectionYaw;
            float    yawError;
            float    pitchError;
            float    rollError;

            float terrainAltitude;
            float dynPressure;
            float velocity;

            //Setup
            targetDirection    = vesselTransform.InverseTransformDirection(targetPosition - velocityTransform.position).normalized;
            targetDirectionYaw = targetDirection;

            terrainAltitude = GetRadarAltitude();
            dynPressure     = (float)vessel.dynamicPressurekPa;
            velocity        = (float)vessel.srfSpeed;

            pitchError = (float)Math.Asin(Vector3d.Dot(Vector3d.back, VectorUtils.Vector3dProjectOnPlane(targetDirection, Vector3d.right))) * Mathf.Rad2Deg;
            yawError   = (float)Math.Asin(Vector3d.Dot(Vector3d.right, VectorUtils.Vector3dProjectOnPlane(targetDirectionYaw, Vector3d.forward))) * Mathf.Rad2Deg;

            //roll
            Vector3 currentRoll = -vesselTransform.forward;
            Vector3 rollTarget;

            upWeighting = pilot.UpWeighting(terrainAltitude, dynPressure, velocity);

            rollTarget = (targetPosition + Mathf.Clamp(upWeighting * (100f - Math.Abs(yawError * 1.6f) - (pitchError * 2.8f)), 0, float.PositiveInfinity) * upDirection) - vessel.CoM;

            rollTarget = Vector3.ProjectOnPlane(rollTarget, vesselTransform.up);

            rollError = VectorUtils.SignedAngle(currentRoll, rollTarget, vesselTransform.right);

            Steer steer = pilot.Simulate(pitchError, rollError, yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);

            s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
            if (s.roll == s.rollTrim)
            {
                s.roll = Mathf.Clamp(steer.roll, -1, 1);
            }
            s.yaw = Mathf.Clamp(steer.yaw, -1, 1);

            //Debug
            dynPressDebug       = dynPressure;
            speedFactorDebug    = dynPressure * 16 / velocity;
            invSpeedFactorDebug = 1 / (speedFactorDebug + Single.Epsilon);
            //------------------
        }
Ejemplo n.º 4
0
        void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
        {
            Vector3d srfVel = vessel.srf_velocity;

            if (srfVel != Vector3d.zero)
            {
                velocityTransform.rotation = Quaternion.LookRotation(srfVel, -vesselTransform.forward);
            }
            velocityTransform.rotation = Quaternion.AngleAxis(90, velocityTransform.right) * velocityTransform.rotation;
            Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;

            Vector3d targetDirection;
            Vector3d targetDirectionYaw;
            //float yawError;
            //float pitchError;
            //float rollError;

            float terrainAltitude;
            float dynPressure;
            float velocity;

            //Setup

            targetDirection    = vesselTransform.InverseTransformDirection(targetPosition - velocityTransform.position).normalized;
            targetDirectionYaw = targetDirection;

            terrainAltitude = GetRadarAltitude();
            dynPressure     = (float)vessel.dynamicPressurekPa;
            velocity        = (float)vessel.srfSpeed;

            upWeighting = pilot.UpWeighting(terrainAltitude, dynPressure, velocity);

            //Setting forced behavior for testing while there is no toggle button
            flightMode.SetBehavior(1);

            //Calculating errors
            ErrorData behavior = flightMode.Simulate(vesselTransform, targetDirection, targetDirectionYaw, targetPosition, upDirection, upWeighting, vessel);

            //Controlling
            Steer steer = pilot.Simulate(behavior.pitchError, behavior.rollError, behavior.yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);

            //Piloting
            s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
            if (s.roll == s.rollTrim)
            {
                s.roll = Mathf.Clamp(steer.roll, -1, 1);
            }
            s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
        }
Ejemplo n.º 5
0
        public Steer Simulate(float pitchError, float rollError, float yawError, UnityEngine.Vector3 angVel, float terrainAltitude, float timestep, float dynPress, float vel)
        {
            float speedFactor = vel / dynPress / 16; //More work needs to be done to sanitize speedFactor

            if (speedFactor > 1.5f)
                speedFactor = 1.5f;

            float trimFactor = (float)Math.Sqrt(speedFactor);

            if (!MouseAimSettings.FARLoaded)                //TODO: definitely remove this in favor of adaptation that works with FAR
                AdaptGains(pitchError, rollError, yawError, angVel, terrainAltitude, timestep, dynPress, vel, trimFactor);

            float steerPitch = pitchPID.Simulate(pitchError, angVel.x, pIntLimit * trimFactor, timestep, speedFactor);
            float steerRoll = rollPID.Simulate(rollError, angVel.y, rIntLimit, timestep, speedFactor);
            if (pitchPID.IntegralZeroed)        //yaw integrals should be zeroed at the same time that pitch PIDs are zeroed, because that happens in large turns
                yawPID.ZeroIntegral();
            float steerYaw = yawPID.Simulate(yawError, angVel.z, yIntLimit, timestep, speedFactor);

            Steer steer = new Steer (steerPitch, steerRoll, steerYaw);

            return steer;
        }