Tilt Heading Berechnung arbeitet nicht korrekt

[Ritchie]

Hallo Zusammen,

ich habe mir jetzt die ganze Formel nochmals angesehen und wenn ich das richtig sehe stimmt die Umsetzung der Formel
laut Application note.

Quellcode:

//==================================================================
//        Get the Pitch / Row and heading
//==================================================================

void getAtomicTiltHeading(void)
{
float    Atomic_Xnorm;
float    Atomic_Ynorm;
float    temp;

temp=sqrt(Atomic_accel[X] * Atomic_accel[X]+ Atomic_accel[Y] * Atomic_accel[Y] + Atomic_accel[Z] * Atomic_accel[Z]);

Atomic_Xnorm = Atomic_accel[X]/temp;
Atomic_Ynorm = Atomic_accel[Y]/temp;

Angle[PITCH] = asin(-Atomic_Xnorm);
Angle[ROLL]= asin(Atomic_Ynorm/cos(Angle[PITCH]));

float AccXcomp = LM303_magnet[X]*cos(Angle[PITCH])+LM303_magnet[Z]*sin(Angle[PITCH]);
float AccYcomp =  LM303_magnet[Y]*sin(Angle[ROLL])*sin(Angle[PITCH])+LM303_magnet[Y]*cos(Angle[ROLL])-LM303_magnet[Z]*sin(Angle[ROLL])*cos(Angle[PITCH]);

Angle[HEADING] = atan2(AccYcomp,AccXcomp);
}

Formel:


Eigentlich kann es jetzt nur noch an den Werten der Sensoren und deren Ausrichtung liegen (oder ?) .

Keine eine Idee wo mein Fehler sein könnte ?

Viele Grüße

R.