[C] Obsługa magnetometru HMC5883L

[wooojtekk]

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.

void calib_hmc5883l(void){

//------- Kalibracja magnetometru HMC5883L ------------

if (mx<kalib.min_mx) kalib.min_mx=mx;
if (kalib.max_mx<mx) kalib.max_mx=mx;

if (my<kalib.min_my) kalib.min_my=my;
if (kalib.max_my<my) kalib.max_my=my;

if (mz<kalib.min_mz) kalib.min_mz=mz;
if (kalib.max_mz<mz) kalib.max_mz=mz;

kalib.mx_off = (kalib.max_mx + kalib.min_mx)/2;
kalib.my_off = (kalib.max_my + kalib.min_my)/2;
kalib.mz_off = (kalib.max_mz + kalib.min_mz)/2;

//------- Wyrażenie wartości w mG ------------------------------
kalib.norm_mx = (mx - kalib.mx_off)*0.00092;
kalib.norm_my = (my - kalib.my_off)*0.00092;
kalib.norm_mz = (mz - kalib.mz_off)*0.00092;


//-----------obliczenie kąta obrotu bez kompensacji - yaw-----------

yaw = atan2(kalib.norm_my,kalib.norm_mx)*180/M_PI;

//-----------obliczenie kąta obrotu z kompensacją - yaw2  -----------

//	 wartośc kąta roll i pitch obliczona na podstawie danych z akcelerometru
//	 roll = ((atan2(ay,az))*180/M_PI);
//	 pitch = -(atan2(ax,sqrt(ay*ay + az*az))*180)/M_PI;

sinroll = sin(roll*M_PI/180);				//wyznaczenie sin(roll),
cosroll = cos(roll*M_PI/180);				//wyznaczenie cos(roll),
sinpitch = sin(pitch*M_PI/180);				//wyznaczenie sin(pitch),
cospitch = cos(pitch*M_PI/180);				//wyznaczenie cos(pitch),

yaw2 = atan2(kalib.mx_off*sinroll*sinpitch + kalib.my_off*cosroll - kalib.mz_off*sinroll*cospitch,kalib.mx_off*cospitch + kalib.mz_off*sinpitch)*180/M_PI;
}