Aktyw Forum

Zarejestruj się na forum.ep.com.pl i zgłoś swój akces do Aktywu Forum. Jeśli jesteś już zarejestrowany wystarczy, że się zalogujesz.

Sprawdź punkty Zarejestruj się

Magnetometr. Kompensacja kąta wychylenia

fasolus
-
-
Posty: 1
Rejestracja: 27 lip 2019, o 12:14

Magnetometr. Kompensacja kąta wychylenia

Postautor: fasolus » 27 lip 2019, o 12:19

Cześć.

Ma ktoś z was pomysł, dlaczego taka próba kompensacji kąta wychylenia magnetometru nie przynosi rezultatu ? Algorytm działa prawidłowo tylko w przypadku, kiedy magnetometr nie jest odchylony od poziomu (Roll = 0, Pitch = 0).

float Roll = DataStructure->Accelerometer_Roll;
float Pitch = DataStructure->Accelerometer_Pitch;

float cosRoll = cosf(Roll * (M_PI / 180)), cosPitch = cosf(Pitch * (M_PI / 180));
float sinRoll = sinf(Roll * (M_PI / 180)), sinPitch = sinf(Pitch * (M_PI / 180));

float H_X = (DataStructure->Magnetometer_X_uT * cosPitch) + (DataStructure->Magnetometer_Y_uT * sinRoll * sinPitch) + (DataStructure->Magnetometer_Z_uT * cosRoll * sinPitch);
float H_Y = (DataStructure->Magnetometer_Y_uT * cosRoll) - (DataStructure->Magnetometer_Z_uT * sinRoll);

DataStructure->Magnetometer_Yaw = atan2f(H_Y, H_X) * (180 / M_PI) + MAGNETIC_DECLINATION;

Zmienna Roll zawiera się w zakresie <-180, +180>, Pitch tak samo. Funkcje trygonometryczne również dają prawidłowe wartości <-1,1> w zależności od Roll i Pitch.

Wróć do „Sterowanie, kontrola, czujniki i syst. wizyjne”

Kto jest online

Użytkownicy przeglądający to forum: Obecnie na forum nie ma żadnego zarejestrowanego użytkownika i 1 gość