Układ realizowany jest na uC Atmega8. Poniżej przedstawiam funkcję odpowiedzialną za obliczanie kąta obrotu.[syntax=c]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;
}[/syntax]
Statystyki: Napisane przez wooojtekk — 24 lis 2017, o 22:41
]]>