Kipp- und Neigungswinkel nur aus 3D Magnetfelddaten
Hallo
ich habe eine ziemlich allgemeine Frage bezüglich der Nutzung von 3D
Magnetfeldsensoren:
- kann ich mit den Informationen von solch einem Sensor Kipp(pitch) und
Neigungswinkel(roll) ermitteln
Ich bau gerade eine Lageregelung für eine mehrfach höhenverstellbare
Plattform und habe bereits ein fusioniertes Signal von einem 3D-Gyro und
einem 3D-Beschleunigungssensor.
Um die Genauigkeit und die Ausfallsicherheit zu erhöhen will ich mit
einem 3D-Kompass (Magnetfeldsensor) Kipp- und Neigungswinkel ebenfalls
berechnen und dann wieder fusionieren (vllt inform einer gewichteten
Summe o.ä.).
Auf die Idee bin ich gekommen da ich denke das Beschleunigungssensoren
und Magnetfeldsensoren dass selbe machen bis auf die Verdrehnung der
Koordinatensysteme.
Da ich bereits einen funktionierenden Algorithmus für die
Winkelberechnung durch den Beschleunigungssensor habe wollte ich diesen
gern einfach so anpassen, dass ich den Verdrehungswinkel (Inklination
ca. 67°) berücksichtige.
Auf meiner Suche nach möglichen Ansätzen habe ich viele Threads zur
Ermittlung der Orientierung bzgl. Nordausrichtung mithilfe von
Magnetfeldsensoren und zur Tiltkompensation unter Nutzung von
Beschleunigungssensoren gefunden. Deshalb bin ich mir nicht mehr sicher
ob ich bei meiner obigen Überlegung bzgl. des Zusammenhangs zwischen
Beschleunigungs- und Magnetfeldsensoren nicht doch einen prinzipiellen
Denkfehler habe.
zur Vollständigkeit hier der Code für den ADXL345 welchen ich für einen
HMC5843 anpassen möchte:
Code:
void convert_ADXL(Uint16* data)
{
//>>>>>>>Conversion begin<<<<<<<<
ADXL_dx=(((int)data[0])|((int)data[1]<<8));
ADXL_dy=(((int)data[2])|((int)data[3]<<8));
ADXL_dz=(((int)data[4])|((int)data[5]<<8));
//>>>>>>>Conversion end<<<<<<<<<<
}
void ADXL_angle()
{
//range fitting
double const ADXL_x_norm = 0.955224; // dx : 280 ... - 256 = 536 -> 512/536 = 0.955224;
double const ADXL_y_norm = 0.966037; // dy : 264 ... - 266 = 530 -> 512/530 = 0.966037;
double const ADXL_z_norm = 0.994175; // dz : 248 ... - 267 = 515 -> 512/515 = 0.994175;
//zero-point shifting
double const ADXL_x_null = -12.0000; // x : (280 + x_null) * x_norm = 256;
double const ADXL_y_null = 1.0002; // y : (264 + y_null) * y_norm = 256;
double const ADXL_z_null = 9.4999; // z : (248 + z_null) * z_norm = 256;
ADXL_xn = ADXL_dx;
ADXL_xn += ADXL_x_null;
ADXL_xn = (int)(double) ADXL_xn * ADXL_x_norm;
ADXL_gxn = ADXL_xn*D2G;
ADXL_yn = ADXL_dy;
ADXL_yn += ADXL_y_null;
ADXL_yn = (int)(double) ADXL_yn * ADXL_y_norm;
ADXL_gyn = ADXL_yn*D2G;
ADXL_zn = ADXL_dz;
ADXL_zn += ADXL_z_null;
ADXL_zn = (int)(double) ADXL_zn * ADXL_z_norm;
ADXL_gzn = ADXL_zn*D2G;
one_g=sqrt((ADXL_gxn*ADXL_gxn)+(ADXL_gyn*ADXL_gyn)+(ADXL_gzn*ADXL_gzn));
ADXL_pitch = R2D*(atan(ADXL_gxn/(sqrt((ADXL_gyn*ADXL_gyn)+(ADXL_gzn*ADXL_gzn)))));
ADXL_roll = R2D*(atan(ADXL_gyn/(sqrt((ADXL_gxn*ADXL_gxn)+(ADXL_gzn*ADXL_gzn)))));
ADXL_yaw = R2D*(atan(ADXL_gzn/(sqrt((ADXL_gxn*ADXL_gxn)+(ADXL_gyn*ADXL_gyn)))));
}
Vielen Dank für Ideen/Anregungen im Voraus.
Stephan
