Das ist Pseudocode, also gar keine Sprache
Ich dachte mir doch, dass es an der Seriellen liegt...
Nunja, da der MPU ja einen 3Achs ACC eingebaut hat, könntest du z.b. die Daten fusionieren (mithilfe eines Komplementärfilters oder Kalman oder...).
Oder aber du nutzt diesen Winkel ohne den ACC, allerdings wird der nicht sehr lange stabil sein.
Was genau hast du den vor?

Gruß
Chris