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);
}
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);
}
public byte[] Serialize()
{
return(FrameInput.Serialize(this));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// <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));
}
internal void Initialize()
{
Update();
LastInput = CurrentInput;
}
public override void Deserialize(Deserializer reader)
{
input = reader.ReadRef(ref input);
}
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]);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// <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));
}
private void readFrameUpdateData(NetIncomingMessage inMsg)
{
FrameInput input = new FrameInput();
input.deserialize(inMsg);
clientWorld.addFrameInputData(input);
}
/// <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);
}
public bool PushEndOfStream()
{
var NewFrame = new FrameInput();
return(PushFrameData(NewFrame));
}
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);
}