XMega Quadrocopter BMP085 Werte springen

Hi,

ich bin momentan dabei, einen BMP085 in meinen Quadrocopter einzubauen und zu programmieren. Prinzipiell funktioniert es auch schon, also die Werte werden zumindest vom Sensor ausgelesen. Sie sind zwar alle negativ, aber eine Änderung der Höhe sieht man auch im negativen Wert, also stört mich das nicht. Der Sensor wird alle paar ms in einer ISR abgefragt (nur die Register werden über I2C ausgelesen) und die Höhe und Temperatur werden dann im Mainloop berechnet. Soweit so gut. Es gibt in meinem Code 3 States, State 0 bedeutet Motoren aus, State 1 bedeutet Winkelgeschwindigkeitssteuerung und State 2 bedeutet Hover/Level Mode, also Winkelsteuerung. In State 0 und State 1 funktioniert alles, aber sobald ich in State 2 schalte, springen die Werte um den Faktor 10 des eigentlichen Wertes hin und her... Da bräuchte ich jetzt eure Hilfe!
Ich dachte schon an einen Stack-Overflow, also hab ich die Stacks von 100 auf 150 vergrößert, aber das bringt auch nichts. Alle Variablen, die von den BMP085 Routinen verwendet werden, werden sonst nirgends verwendet. In der Timer-ISR, welche für die Regelung zuständig ist, werden nur die Register des BMP085 ausgelesen. Diese stimmen immer(!!), also muss der Fehler irgendwo beim Aufruf der Sub Calc_height() im Mainloop liegen.... Nur WO?
Der Code ist im Anhang, da er zu lange ist für den Thread.

Wenn jemand ne Idee hat, immer raus damit, würde mich sehr freuen :D

Gruß & Vielen Dank
Chris