Esempio n. 1
0
    void Update()
    {
        if (!tracking)
        {
            return;
        }
        if (WaypointReached)
        {
            if (++waypointIndex > waypointList.Count - 1)
            {
                waypointIndex = 0;                 //reset the index if it reaches the end of the list
            }
        }
        currentInput = new FrameInput();
        currentInput.Reset();         //change to STAB mode and baromode = true

        //If we do not face the target, turn
        Vector3 proj = Vector3.ProjectOnPlane(NextWaypoint - drone.transform.position, Vector3.up);
        float angle  = Vector3.Angle(proj, -Vector3.right);
        drone.setYaw = Vector3.Angle(proj, Vector3.back) < 90 ? -angle : angle;

        //If we are not on the same height as the target, adjust throttle
        currentInput.Throttle = Mathf.Clamp(pids[PIDs.THRUST].GetPID(NextWaypoint.y - drone.setAltitude), -1, 1);

        //pitch and roll to adjust the horizontal velocity vector
        //Vector3 addedVector = Vector3.ClampMagnitude(SecondNextWaypoint - NextWaypoint, 2.5f)*Mathf.Clamp01(1 - proj.magnitude/4);
        //Vector3 diff = (proj + 2*proj.normalized + addedVector);

        Vector3 diff = proj.normalized * 4;
        diff -= Vector3.ProjectOnPlane(drone.Velocity, Vector3.up);
        diff  = drone.transform.InverseTransformDirection(diff);
        //clampLimit = Mathf.Clamp(10/diff.magnitude + .3f, -1, 1);
        currentInput.Pitch = Mathf.Clamp(pids[PIDs.PITCH].GetPID(diff.x), -clampLimit, clampLimit);
        currentInput.Roll  = Mathf.Clamp(pids[PIDs.ROLL].GetPID(diff.z), -clampLimit, clampLimit);
        DebugExtension.DebugArrow(drone.transform.position, proj, Color.blue);
        DebugExtension.DebugArrow(drone.transform.position, drone.Velocity, Color.red);
    }
Esempio n. 2
0
        public int PushFrameData(FrameInput Frame)
        {
            if (!ThreadedDecoding)
            {
                return(PopH264_PushData(Instance.Value, Frame.Bytes, Frame.Bytes.Length, Frame.FrameNumber));
            }

            if (InputThread == null)
            {
                InputQueue  = new List <FrameInput>();
                InputThread = new System.Threading.Thread(new System.Threading.ThreadStart(ThreadPushQueue));
                InputThread.Start();
            }

            //	add data and wake up the thread in case we need to
            lock (InputQueue)
            {
                InputQueue.Add(Frame);
                //PushByteThread.Resume();
            }

            //	check for
            return(InputThreadResult.HasValue ? InputThreadResult.Value : 0);
        }
Esempio n. 3
0
 public byte[] Serialize()
 {
     return(FrameInput.Serialize(this));
 }
Esempio n. 4
0
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// <summary>
 /// Try to set the input that has been confirmed by the character.
 /// </summary>
 /// <returns>Whether the input could be set successfully.</returns>
 /// <param name="frame">Frame.</param>
 /// <param name="character">Character.</param>
 /// <param name="characterInput">Character Input.</param>
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 public bool TrySetConfirmedInput(int player, long frame, FrameInput characterInput)
 {
     return(this.TrySetConfirmedInput(player, frame, characterInput, false));
 }
Esempio n. 5
0
 internal void Initialize()
 {
     Update();
     LastInput = CurrentInput;
 }
Esempio n. 6
0
 public override void Deserialize(Deserializer reader)
 {
     input = reader.ReadRef(ref input);
 }
Esempio n. 7
0
    void Update()
    {
        input = inputManager.CurrentInput;

        //Update flight modes
        if (flightMode != input.FlightMode)
        {
            if (input.FlightMode == ControlMode.STAB)
            {
                setYaw = Yaw;                 //if turning stab mode on, set yaw to current
            }
            flightMode = input.FlightMode;
        }
        if (baroMode != input.BaroMode)
        {
            setAltitude = Altitude;             //if turning baro mode on, set altitude to current
            baroMode    = input.BaroMode;
        }

        flightModeText.text = string.Format("{0}{1}", baroMode ? "BARO\n" : "", flightMode.ToString());

        //Gyro information
        Vector3 currentGyro = Mathf.Rad2Deg * transform.InverseTransformDirection(rb.angularVelocity);

        currentGyro.z = -currentGyro.z;

        velocityText.text    = string.Format("({0:F3}, {1:F3})\nVert.: {2:f3}", Velocity.x, Velocity.z, Velocity.y);
        orientationText.text = string.Format("Actual: {0:F3}\nSet: {1:F3}\nPitch: {2:F5}\nRoll: {3:F5}\nYaw: {4:F5}\nSet: {5:F5}",
                                             Altitude, setAltitude, Pitch, Roll, Yaw, setYaw);

        //Throttle
        if (baroMode)
        {
            setAltitude += input.Throttle * BARO_RATE * Time.deltaTime;
            if (Inverted)
            {
                throttle = 0;                 //No base throttle if flipped
            }
            else
            {
                float pid = pids[PIDs.BARO].GetPID(setAltitude - Altitude);
                throttle = Mathf.Clamp(hoverThrottle + pid, 0, 1);
            }
        }
        else
        {
            throttle = Mathf.Clamp(input.Throttle, 0, 1);
        }

        //Pitch, roll, and yaw PIDs
        float pitchOut = 0, rollOut = 0, yawOut = 0;

        //TODO FIXME yaw turning (reversing direction specifically) causes altitude gain in stab mode
        if (flightMode == ControlMode.STAB)
        {
            //FIXME get these values working so that they can wrap with the pids //Not necessary except for exceptional cases
            //TODO Autolevel if upside-down in stab mode
            float pitch_stab_output = pids[PIDs.PITCH_STAB].GetPID(input.Pitch * STAB_PITCH_MAX - Pitch);
            float roll_stab_output  = pids[PIDs.ROLL_STAB].GetPID(input.Roll * STAB_ROLL_MAX - Roll);
            float yaw_stab_output;
            if (input.Yaw != 0)
            {
                yaw_stab_output = input.Yaw * STAB_YAW_RATE;
                setYaw          = Yaw;        //TODO change this to be ahead by an amount depending on the turn rate to remove slingshotting
            }
            else
            {
                yaw_stab_output = pids[PIDs.YAW_STAB].GetPID(AngleDifference180(Yaw, setYaw));
            }

            pitchOut = pids[PIDs.PITCH_RATE].GetPID(currentGyro.z - pitch_stab_output);
            rollOut  = pids[PIDs.ROLL_RATE].GetPID(currentGyro.x - roll_stab_output);
            yawOut   = pids[PIDs.YAW_RATE].GetPID(currentGyro.y - yaw_stab_output);
        }
        else if (flightMode == ControlMode.ACRO)
        {
            pitchOut = pids[PIDs.PITCH_RATE].GetPID(currentGyro.z - input.Pitch * ACRO_PITCH_RATE);
            rollOut  = pids[PIDs.ROLL_RATE].GetPID(currentGyro.x - input.Roll * ACRO_ROLL_RATE);
            yawOut   = pids[PIDs.YAW_RATE].GetPID(currentGyro.y - input.Yaw * ACRO_YAW_RATE);
        }

        debugText.text    = string.Format("X: {0:F3}\nY: {1:F3}", input.Roll, input.Pitch);
        throttleText.text = string.Format("Throttle: {0:F0}\tPitchOut: {1:F5}\tRollOut: {2:F5}\tYawOut:{3:F5}", throttle, pitchOut, rollOut, yawOut);

        //Mixer

        //	  ^
        //	  |
        //	2	1
        //	  X
        //	3	0
        //

        float[][] mixTable =
        {
            new float[] { 1.00f,  1.00f,  1.00f, -1.00f },
            new float[] { 1.00f,  1.00f, -1.00f,  1.00f },
            new float[] { 1.00f, -1.00f, -1.00f, -1.00f },
            new float[] { 1.00f, -1.00f,  1.00f,  1.00f },
        };

        float[] thrust = new float[4];
        //y tilts forewards and back, x left and right

        for (int i = 0; i < 4; i++)
        {
            float[] mix = mixTable[i];
            thrust[i] = Mathf.Clamp(mix[0] * throttle + mix[1] * rollOut + mix[2] * pitchOut + mix[3] * yawOut, 0, 1);
        }

        mousePosText.text = string.Format("{0:F3}\t\t{1:F3}\n{0:F3}\t\t{1:F3}",
                                          thrust[2], thrust[1],
                                          thrust[3], thrust[0]);

        for (int i = 0; i < props.Length; i++)          //Set speed of each propeller
        {
            props[i].SetSpeedPercent(thrust[i]);
        }
    }
Esempio n. 8
0
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// <summary>
 /// Try to set the input that has been confirmed by the player.
 /// </summary>
 /// <returns>Whether the input could be set successfully.</returns>
 /// <param name="frame">Frame.</param>
 /// <param name="playerInput">Player Input.</param>
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 public bool TrySetConfirmedInput(long frame, FrameInput playerInput)
 {
     return(this.TrySetConfirmedInput(frame, playerInput, false));
 }
Esempio n. 9
0
        private void readFrameUpdateData(NetIncomingMessage inMsg)
        {
            FrameInput input = new FrameInput();
            input.deserialize(inMsg);

            clientWorld.addFrameInputData(input);
        }
Esempio n. 10
0
        /// <summary>
        /// Send an update to the server that acknowledges that this client completed an update frame
        /// and informs it of any input that was given by this client
        /// </summary>
        internal void sendFrameUpdate(FrameInput input)
        {
            NetOutgoingMessage outMsg = netClient.CreateMessage();

            outMsg.Write((byte)NetDataType.FrameUpdate);
            input.serialize(outMsg);

            netClient.SendMessage(outMsg, NetDeliveryMethod.ReliableOrdered);
        }
Esempio n. 11
0
        public bool PushEndOfStream()
        {
            var NewFrame = new FrameInput();

            return(PushFrameData(NewFrame));
        }
Esempio n. 12
0
        private void relayFrameInput(NetIncomingMessage inMsg)
        {
            FrameInput input = new FrameInput();
            input.deserialize(inMsg);

            //Write it to a new packet
            NetOutgoingMessage outMsg = netServer.CreateMessage();
            outMsg.Write((byte)NetDataType.FrameUpdate);
            input.serialize(outMsg);

            netServer.SendToAll(outMsg, NetDeliveryMethod.ReliableOrdered);
        }