Stm32F3 Disco: eKompass Firmware: Bug ?

Hallo,
ich wollte eine Platform bauen, die sich zu einer Himmelsrichtung 
ausrichten kann (ekompass). Ich habe das Stm32f3 Discovery Board als 
Vorlage genommen und mir daher den gleichen Chipset bei den Chinesen 
bestellt. Um es mir einfach zu machen, wollte ich einfach die 
Kompass-Firmware vom Stm32f3disco übernehmen.

Das ging einigermaßen, solange das Board flach auf dem Tisch lag. Sobald 
ich es ein bisschen ge'tilt habe, ist der Ausgangswert gesprungen! Die 
Tilt-Korrektur in der ST-Firmware scheint falsch zu sein. Im Internet 
konnte ich nichts viel dazu finden. Es hat mich gewundert, dass es wohl 
noch niemanden aufgefallen ist.

Anpassungen:
- Die Werte, die mittels Wurzel bestimmt werde, können nicht kleiner 
null werden. Es gibt aber Bedingungen, die auf <0 testen. Daher habe ich 
noch eine Zeile hinzugefügt, die die Winkel negative machen kann.
- Die atan2 Berechung für heading habe ich auch geändert.

Mit den Änderungen scheint es zu tun. Falls jemand Zeit hat, dann bitte 
mal gegenchecken.
Im Anhang ist die Original und die geänderte Datei zum Vergleich.