Witajcie forumowicze

Poszukuję pomocy w obsłudze magnetometru HMC5883L. Mianowicie, chcę na podstawie danych z magnetometru wyznaczyć kąt obrotu z kompensacją przechyleń, wykorzystując dodatkowo układ MPU6050. Udało mi się odczytać dane z obu czujników, jak i obliczyć kąt obrotu bez kompensacji. Natomiast, gdy próbuję obliczyć kąt obrotu z kompensacją to nie wychodzi. Gdy czujnik nie jest przechylany to obliczona wartość kąta jest taka sama jak ta, obliczona bez kompensacji. Jeżeli czujnik przechylę, to obliczona wartość kąta z kompensacją "ucieka". Męczę się z tym problemem już 2 tygodnie i nadal nic. Może ktoś z was mógłby mi pomóc?

Układ realizowany jest na uC Atmega8. Poniżej przedstawiam funkcję odpowiedzialną za obliczanie kąta obrotu.
język c
Musisz się zalogować, aby zobaczyć kod źródłowy. Tylko zalogowani użytkownicy mogą widzieć kod.