[Leopard2K]

Dzień dobry,
próbuje wyznaczyć kąt o jaki został obrócony magnetometr względem początkowej pozycji magnetometru.

Kod:

if (flag==0 )
{yaw1=round(heading2);flag=1;}

Zmienna heading2 to kąty kompasu. Mają stałe wartości w zakresie od 0 do 360 stopnii. W powyższym skrypcie jednorazowo następuje przypisanie zmiennej yaw1.

Kod:

OBROT=heading2-yaw1;
H3_R=OBROT*(PI/180);

x=cos(H3_R)-sin(H3_R);
y=sin(H3_R)+cos(H3_R);

H3_D=atan2(y,x);

H3_D=(H3_D*(180/PI))-45;

H3_D=round(H3_D);

Zmienna H3_D informuje o jaki kąt został obrócony magnetometr względem wartości początkowej.

Niestety w trakcie obrocie po przekroczeniu pewnych wartości, wyniki się nie zgadzają. Tekst : lowering/rising to informacja czy dane rosną czy maleją. 

W załączniku dodałem zdjęcie tego co się dzieje.


Chciałbym poprosić o pomoc w naprawieniu tego problemu. Czyli jak obracam zgodnie z kierunkiem zegara magnetometr to wartości H3_D również nieprzerwanie, prawidłowo rosną. W przeciwną stronę chciałbym uzyskać wartości na minusie.