Hallo Liebe Community,
ich versuche einen neigungskompensierten Kompass mit Hilfe eines
Arduino's, eines Hmc5883l Magnetometers und eines ADXL335 zu bauen.
Leider funktioniert es nicht so wie erwartet und den Fehler kann ich
nicht entdecken...
Zum Beschleunigungssensor ADXL335:
Es ist ein analoger Sensor, weswegen ich zuerst die Volt-Werte lese.
float xAcc = xRead;
float yAcc = yRead;
float zAcc = zRead;
Dann wird ein Offset subtrahiert, damit die X- und Y-Achse auf 0 Volt
liegt, wenn der Sensor HORIZONTAL liegt. Und ein Offset bei zAcc
abgezogen, damit dieser 0 liegt, wenn die Z-Achse horizontal liegt:
float xAcc = (xRead-327);
float yAcc = (yRead-333);
float zAcc = (zRead-334);
Nun noch die Werte skalieren, sodass sie in g (Erdbeschleunigung)
ausgegeben werden:
float xAcc = (xRead-327)/69.00;
float yAcc = (yRead-333)/69.00;
float zAcc = (zRead-334)/69.00;
Liegt der Sensor gerade kommt: X: 0g Y: 0g Z:1g
Liegt die X-Achse oben, kommt: X:1 Y:0 Z:0 usw.
Diese werden nun in Radians umgewandelt:
float pitch = atan(xAcc/sqrt(pow(yAcc,2) + pow(zAcc,2)));
float roll = atan(yAcc/sqrt(pow(xAcc,2) + pow(zAcc,2)));
Wobei diese Formel auch funktioniert: (ich weiß nicht warum)
float rollRadians = asin(yAcc);
float pitchRadians = asin(xAcc);
Eigentlich müsste das alles stimmen.
Zum Magnetometer HMC5883l:
Diesen steuere ich durch eine Bibliothek an, die ich jetzt nicht genauer
erläutere. Ich bekomme dann die "skalierten" Achsen ausgegeben.
MagnetometerScaled scaled = compass.ReadScaledAxis();
float xMag = scaled.XAxis;
float yMag = scaled.YAxis;
float zMag = scaled.ZAxis;
Hier gibt es noch eine Methode, um die "raw" (Rohdaten) der Achsen
auszugeben. Ich denke aber, dass die nicht zu gebrauchen sind. Oder
kennt sich dort jemand aus?
Zur Neigungskompensation:
Nun benutze ich Formeln, die ich aus dem Internet habe.
Von: 6DOF Arduino | PDF | Compass | Accelerometer
float cosRoll = cos(rollRadians);
float sinRoll = sin(rollRadians);
float cosPitch = cos(pitchRadians);
float sinPitch = sin(pitchRadians);
float Xh = scaled.XAxis * cosPitch + scaled.ZAxis * sinPitch;
float Yh = scaled.XAxis sinRoll sinPitch + scaled.YAxis * cosRoll -
scaled.ZAxis sinRoll cosPitch;
float heading = atan2(Yh, Xh);
Diese Formel funktioniert komischerweise nicht. Hebe ich beide Sensoren
auf dem Breadboard an der Seite an, bleiben die "Heading"-Werte nicht
gleich, sondern ändern sich.
Ich habe schon pitch und roll getauscht, die Achsen umgedreht, nichts
hat geholfen.
Auch diese Formel, der Webseite Home - TimZaman.com ,
funktioniert nicht:
xh=cx*cos(ayf)+cy*sin(ayf)*sin(axf)-cz*cos(axf)*sin(ayf);
yh=cy*cos(axf)+cz*sin(axf);
Daher frage ich euch: Wo liegt mein Fehler?
Vielen Dank im Voraus! Hoffentlich konnte jemand Nutzen aus meinen
Beitrag ziehen, da es doch leider wenig Dokumentation zur
Neigungskompensation im Internet gibt...
