public void FixedUpdate() { Vector4 input = new Vector4(); if (typeOfInput == TypeOfInput.GAMEPAD) { var gamepad = Gamepad.current; if (gamepad != null) { input.x = gamepad.rightStick.y.ReadValue(); input.y = gamepad.rightStick.x.ReadValue(); input.z = gamepad.leftStick.x.ReadValue(); input.w = gamepad.leftStick.y.ReadValue(); } } else { input = externalInput; } float currentVerticalVelocity = rb.velocity.y; var angularVelocity = transform.InverseTransformDirection(rb.angularVelocity); float currentRollVelocity = -angularVelocity.z; float currentYawVelocity = angularVelocity.y; float currentPitchVelocity = -angularVelocity.x; float currentPitchAngle = NormalizeAngle(-rb.rotation.eulerAngles.x, -180, 180); float currentRollAngle = NormalizeAngle(-rb.rotation.eulerAngles.z, -180, 180); var velocity = transform.InverseTransformDirection(rb.velocity); float currentforwardVelocity = -velocity.z; float currentrightVelocity = velocity.x; output = currentYawVelocity; float targetVerticalVelocity = input.x * 5f; float targetYawVelocity = input.y * Mathf.Deg2Rad * 200; float targetForwardVelocity = -input.w * 5; float targetRightVelocity = input.z * 5; float targetPitchAngle = forwardVelocityPID.CalculateCurrentAnswer(currentforwardVelocity, targetForwardVelocity); float targetRollAngle = rightVelocityPID.CalculateCurrentAnswer(currentrightVelocity, targetRightVelocity); float targetRollVelocity = rollAnglePID.CalculateCurrentAnswer(currentRollAngle, targetRollAngle); float targetPitchVelocity = pitchAnglePID.CalculateCurrentAnswer(currentPitchAngle, targetPitchAngle); target = targetYawVelocity; float thrust = thurstPID.CalculateCurrentAnswer(currentVerticalVelocity, targetVerticalVelocity); if (ownYawPid) { yaw = yawPID.CalculateCurrentAnswer(currentYawVelocity, targetYawVelocity); } float pitch = pitchPID.CalculateCurrentAnswer(currentPitchVelocity, targetPitchVelocity); float roll = rollPID.CalculateCurrentAnswer(currentRollVelocity, targetRollVelocity); //Debug.Log("++++++++++++++++++++++++++++++++++++++++++++++"); //Debug.Log(currentRollVelocity); //Debug.Log(currentYawVelocity); //Debug.Log(currentPitchVelocity); //Debug.Log("Thurst: " + thrust); //Debug.Log("VerticalVelocity: " + currentVerticalVelocity); //Debug.Log("++++++++++++++++++++++++++++++++++++++++++++++"); MotorMixing(thrust, yaw, pitch, roll); Debug.Log("Inertia Tensor: " + rb.inertiaTensor); Debug.Log("Inertia Tensor Rotation: " + rb.inertiaTensorRotation.eulerAngles); }