void FixedUpdate() { while (operations.Count > 0) { operations.TryDequeue(out Action action); action.Invoke(); } if (motorsArmed) { if (motorsControlMode == "stable") { float t = 50; var target = Quaternion.Euler(targetRotation); if (Quaternion.Dot(gameObject.transform.rotation, target) < 0) { target.w *= -1; target.x *= -1; target.y *= -1; target.z *= -1; } target.x *= -1; target.y *= -1; target.z *= -1; var error = target * gameObject.transform.rotation; float pitchTarget = error.x * t; float yawTarget = error.y * t; float rollTarget = error.z * t; Vector3 cur = transform.InverseTransformDirection(rb.angularVelocity); //localAngularVelocity var rollControl = rollPID.ControlWithLimits(rollTarget, cur.z, Time.fixedTime); var pitchControl = pitchPID.ControlWithLimits(pitchTarget, cur.x, Time.fixedTime); var yawControl = yawPID.ControlWithLimits(yawTarget, cur.y, Time.fixedTime); var depthControl = targetDepth == 0f ? 0 : depthPID.ControlWithLimits(-targetDepth, transform.position.y, Time.fixedTime); SetMotors(rollControl, pitchControl, yawControl, depthControl, velocity.z, velocity.y, velocity.x); } else if (motorsControlMode == "acro") { Vector3 localangularvelocity = transform.InverseTransformDirection(rb.angularVelocity); var rollControl = rollSpeedPID.ControlWithLimits(targetRotationSpeed.z, localangularvelocity.z, Time.fixedTime); var pitchControl = pitchSpeedPID.ControlWithLimits(targetRotationSpeed.x, localangularvelocity.x, Time.fixedTime); var yawControl = yawSpeedPID.ControlWithLimits(targetRotationSpeed.y, localangularvelocity.y, Time.fixedTime); SetMotors(rollControl, pitchControl, yawControl, 0, velocity.z, velocity.y, velocity.x); } } Module.FixedUpdateAll(); }