Magnetometr hmc5883L - błędne odczyty

[us5f]

Mam problem z powyższym magnetometrem. Jest on w konfiguracji jako slave przy mpu6050.

Konfiguracja niby działa, ale zdarzają się problemy - przy ustawieniu zegara I2C na 400kHz w mpu, przykładowy program "kompas"(wyliczam heading) raz działa, a raz nie. Główne objawy są takie: obracam płytką na stole i heading rośnie np. od 80 do 120, a potem z powrotem i tu po przekroczeniu 80 znów idzie do góry. Tak to wygląda.

Dodatkowo przeprowadziłem self test i dopiero za drugim razem odzczyty z osi X były w normie, reszta była OK.

Obecnie zmniejszyłem zegar I2C na 320kHz i na razie program działa, tylko nie wiem na jak długo. Mam przez to rozumieć, że w moim wypadku nie da się dać na max ? czy moduł jest uszkodzony ?

Używam bibliotek Korneliusza Jarzębskiego, tylko dodana została funkcja konfigurująca mpu i hmc, "scalone w jedność" . Płytka została zakupiona w hobbyking - multiwii_32_u4 (czy jakoś tak), obecnie nie ma jej w ofercie.

sensors.h

sensors.cpp

Kompas.zip