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.

[Rabenauge]

Nur raten, da ich das Board nicht kenne: Hard-Iron-Kalibrierung gemacht?
Der bekannte HMC 5883l beispielsweise ist ohne diese nahezu nutzlos. Schon aufm Steckbrett funktioniert er nur halbwegs brauchbar, wenn man ihn schön in die Mitte steckt.

[Ritchie]

Hallo Zusammen,

ich habe das mit folgender Routine bis jetzt immer versucht, bin mir aber nicht sicher ob das so richtig ist.

void    getLSM303_CalibrationValues(void)
{
int16_t        MinValues[3],MaxValues[3];
int16_t        Average[3],i,y,iValue;
float        Average_rad;

    for (i=0;i<3;i++)                                // Setup the Min/max values
        {
        MaxValues[i]= -32760;
        MinValues[i] = 32760;
        }    

    Serial_string_printf("\n\nStart of calibration Move around all directions\n\r");
    Serial_string_printf("Press any key to abort the calibration\n\r");

    read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMENT + ADDR_OUT_X_L_M,LM303_magnet_raw,sizeof(LM303_magnet_raw));

    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_L_M, 0);    // Clear the existing Offset value of the chip
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_H_M, 0);
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_L_M, 0);    // Clear the existing Offset value of the chip
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_H_M, 0);
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_L_M, 0);    // Clear the existing Offset value of the chip
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_H_M, 0);

    for(i=0;i<32000;i++)
        {
        read_LSM303();        
        
//        read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMENT + ADDR_OUT_X_L_M,LM303_magnet_raw,sizeof(LM303_magnet_raw));
        
        if( (i % 100) == 0 )                            //  show every 100 readings
            STATUS_LED_TOOGLE;                            // Show Updating of values    by toggle bit

        if( (i % 1000) == 0 )                            //  show every 1000 readings
            {
            Serial_string_printf("Loop Value : ");    
            Serial_int16_printf(i);    
            Serial_string_printf("\r");    
            }

        for (y=0;y<3;y++)                                // Setup the Min/max values
            {
            if (LM303_magnet[y] > MaxValues[y] )    // Find the max value of the Sensor
                MaxValues[y] = LM303_magnet[y];

            if (LM303_magnet[y] < MinValues[y] )    // Find the min value of the sensor
                MinValues[y] = LM303_magnet[y];
            }
        }    

    for (i=0;i<3;i++)                                    // Average distance from the center
        {
        iValue =((MinValues[i] + MaxValues[i]) / 2);    // get the average distance from the center
        MaxValues[i] = MaxValues[i] - iValue ;            //
        MinValues[i] = MinValues[i] - iValue ;
        
        Serial_string_printf("\r\n Min: ");
        Serial_float_printf(MinValues[i]);
        Serial_string_printf(" Max: ");
        Serial_float_printf(MaxValues[i]);
        }

    Average_rad =0.0;                                                    
    for (i=0;i<3;i++)                                // Setup the Min/max values
        {
        Average[i] = (MaxValues[i] + (-1*MinValues[i]) / 2 );        
        LM303_magnet_offset[i] =  (MaxValues[i] + MinValues[i]) / 2 ;
        Average_rad += (float) Average[i];
        }

    Average_rad /= 3.0;

//Finally calculate the scale factor by dividing average radius by average value for that axis.

    for (i=0;i<3;i++)
        {
        LM303_magnet_scaling[i] = Average_rad / Average[i];
        }
    
// Store the final values in the chip

    initLSM303_Offset();
    cli();    
    eeprom_write_block((void*)&LM303_magnet_scaling, (void *) LSM303_MAG_GAIN, sizeof(LM303_magnet_scaling));
    eeprom_write_block((void*)&LM303_magnet_offset, (void *) LSM303_MAG_OFFSET, sizeof(LM303_magnet_offset));
    sei();
    
// Gain of the value

    Serial_string_printf("\n\n\rCalibration is finished\n\n\r");
}

Init des Offset vom Chip direkt:

void    initLSM303_Offset(void)
{
    int8_t        LowByte,HighByte;

    LowByte        = (int8_t) (LM303_magnet_offset[0] & 0xFF);
    HighByte    = (int8_t) (LM303_magnet_offset[0] >> 8);
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_L_M, LowByte);    // Store the new value of the X offset
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_H_M, HighByte);
    LowByte        = (int8_t) (LM303_magnet_offset[1] & 0xFF);
    HighByte    = (int8_t) (LM303_magnet_offset[1] >> 8);
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_L_M, LowByte);    // Store the new value of the Y offset
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_H_M, HighByte);
    LowByte        = (int8_t) (LM303_magnet_offset[2] & 0xFF);
    HighByte    = (int8_t) (LM303_magnet_offset[2] >> 8);
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_L_M, LowByte);    // Store the new value of the Z offset
    write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_H_M, HighByte);
}

gelesen wird dann hiermit

void        read_LSM303(void)
{
    int8_t    i;

    read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMENT + ADDR_OUT_X_L_M,LM303_magnet_raw,sizeof(LM303_magnet_raw));
    read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMENT + ADDR_OUT_X_L_A,LM303_accel_raw,sizeof(LM303_accel_raw));

    for(i=0;i<3;i++)
    {
        LM303_magnet[i] = (float)(LM303_magnet_raw[i]) * LM303_magnet_scaling[i] * LSM303D_M_GN_2MG_GAUSS;
        LM303_accel[i] = LM303_accel_raw[i] * LSM303D_LA_SO_2G;
    }
}

Wenn ich dann die grafische Kontrolle mache, sehe ich eigentlich, das es noch nicht sauber zentriert ist.


Wo ist mein Fehler ?

Ich kann die Werte auch direkt eingeben und komme aber nicht zu einem "sauberen" Kreis" in der Grafik.

In welcher Einheit arbeitet Ihr eigentlich für die einzelnen Sensoren ?
mg und mgauss oder in g und gauss

Viele Grüße

R.

[Rabenauge]

Eine Hard-Iron-Kalibrierung kannst du nicht so einfach in Software machen, das funktioniert nicht.
Dabei nimmt man zuerst mal die Störungen auf, indem man das Ganze in allen drei Raumachsen ausmisst.
Aus diesen Daten können dann Korrekturwerte berechnet werden, und _die_ werden dann in der eigentlichen Software benutzt.
Alles andere ergibt keine brauchbaren Ergebnisse.
Schau dir _das_ mal an: http://diydrones.com/profiles/blogs/...on-for-dummies

[Ritchie]

Vielen Dank für den Link.

Hier werde ich mir das ganze mal ansehen und versuchen auf meinen Chip anzuwenden.

Gruss R.

[Ritchie]

Hallo Sly,


hast Du den "Magviewer" ans rennen bekommen. Bei mir läuft zwar das Berechnungsprogramm, aber nicht der MagViewer. Ich habe das in einer VM laufen unter Windows 7 Pro 64.

Leider konnte ich keinen Quellcode vom Magviewer finden.

Habe es auch mal mit der Methode von

http://www.germersogorb.de/html/kali..._hcm5883l.html

probiert und habe hier das Problem, das die Korrekturwerte nicht den gewünschten Effekt haben. Ursache Suche ich noch.

Viele Grüße

R.

[Rabenauge]

Der MagViewer _ging_ unter XP problemlos.
Da man den aber nun nicht unbedingt braucht, hab ich das nicht mehr weiter verfolgt, da ich komplett auf Linux umgestiegen bin- in der VirtualBox lief der auch nicht.
Muss ich mal testen, bei Gelegenheit. Ich starte Windows nur noch ungerne, ich komm da immer vor Lachen nich zu was vernünftigem.
Kann also bissel dauern...letztendlich aber ist der nicht wichtig, man kann sich ja die Kursdaten mal eben seriell ausgeben lassen und schauen, ob das hin kommt.
Hoch präzise wurden die Kompass-Ausgaben bei mir auch nie (+-3 Grad), aber wenn man zusätzlich nen GPS hat, braucht man die auch nicht genauer.