Wie wärs, wenn Du die Gyro Abfragen in einen Timer Interrupt legst, der alle 5 bis 10ms ausgelöst wird.
Dort zyklisch die Gyro Werte addieren und in der Hauptroutine die Position daraus berechnen.
Komplexe Berechnungen sollten da aber dann vermieden werden ( Zeitfaktor ).

Zum anderen würde ich statt der float Variablen lieber int Typen wählen, wo das möglich ist, weil die ein Controller wesentlich schneller verrechnen kann.