MPU 92.65 Winkel berechnen

[Marco (Gast)]

Einen wunderschönen guten Abend wünsche ich euch. Probiere gerade einen 
Gyrosensor auszulesen und einen Winkel zu bestimmen. Mit dem 
Beschleunigungssensor vom Board geht es soweit auch wunderbar, aber 
leider schaffe ich das mit dem Gyrosensor nicht.
Als Sensor habe ich den MPU92.65.

Das mit dem Offset klappt denke ich so weit auch ganz gut. Jedoch hängt 
es gerade mit dem drauf Rechnen der Winkelbeschleunigungswerte. Hat von 
euch eventuell jmd einen Tipp oder Anregung.

#include "mbed.h"
#include "stdio.h"

Serial pc(SERIAL_TX, SERIAL_RX);
SPI spi(PE_6,PE_5,PE_2);  //mosi,miso,sclk
DigitalOut ncs(PE_4);    //ssel

int main()
{
    spi.format(8,0);
    spi.frequency(1000000); 
    
    int16_t z_high, z_low;
    int16_t z_summe_off;
    int16_t x_off, y_off;
    float gz, zz_summe_off, z_off;
    int16_t i;
    
    pc.printf("\n\r");
    
    /****************//******/
    /*Initialisieren*//*Gyro*/
    /****************//******/
    
    ncs=0;
    spi.write(0x1B);     //Gyro_CONFIG  write  
    spi.write(0x18);     //Skalierung    00=+250dps;08=+500dps;10=+1000dps;18=+2000dps
    ncs = 1;                
    wait_ms(1); 
    
    ncs = 0;
    spi.write(0x6B);     //Standby aus
    spi.write(0x00);     
    ncs = 1;               
    wait_ms(1000); 
    
    z_summe_off = 0;

    /********//******/
    /*Offset*//*Gyro*/
    /********//******/ 
    
    for(i = 1;i <= 4000; i++)
    {
        ncs = 0;
        spi.write(0xc7);            //Z_OUT_H
        z_high = spi.write(0x0);
        ncs = 1;
        wait_ms(1); 
           
        ncs = 0;
        spi.write(0xc8);            //Z_OUT_L
        z_low = spi.write(0x0);
        ncs = 1;
        wait_ms(1); 
            
        z_summe_off = z_low + z_high << 8;
        zz_summe_off += z_summe_off;
    }
    pc.printf("\nOffset1:\n\rZ = %2.2f\n\r", zz_summe_off/4000.00);
    z_off = zz_summe_off/4000.00;
    pc.printf("\nOffset2:\n\rZ = %2.2f\n\r", z_off);
    /********//******/
    /*Messen*//*Gyro*/
    /********//******/     
    while(1)
    {  
        ncs = 0;
        spi.write(0xc7);            //Z_OUT_H
        z_high = spi.write(0x0);
        ncs = 1;
        wait_ms(1); 
           
        ncs = 0;
        spi.write(0xc8);            //Z_OUT_L
        z_low = spi.write(0x0);
        ncs = 1;
        wait_ms(1); 
                     
        z_summe_off = z_low + z_high << 8;  //Werte addieren
        pc.printf("Z_summe_off = %#6ld\t", z_summe_off);
        gz += (z_summe_off + z_off);
   
        pc.printf("Z = %2.2f\t\r", gz/22.2);
    }
}

[jemand (Gast)]

Ein Gyro misst die Winkelgeschwindigkeit. Um auf einen Winkel zu kommen, 
muss über die Zeit integriert (aka aufsummiert) werden, aber eine 
Multiplikation mit einer Zeitdifferenz sehe ich in deinem Code nirgens.

[Yalu X. (yalu) (Moderator)]

zz_summe_off wird anfangs nicht auf 0 gesetzt. Dadurch ist der
ermittelte Offset möglicherweise falsch. Auch gz sollte initialisiert
werden, um nachvollziehbare Ergebnisse zu erhalten. Vielleicht haben
beide Variablen anfangs zufälligerweise den Wert 0, aber garantiert ist
das in C nicht.

Das eigentliche Problem liegt aber hier:

        gz += (z_summe_off + z_off);

Du musst den Offset nicht addieren, sondern subtrahieren:

        gz += (z_summe_off - z_off);

Des Weiteren vermisse ich hier (wie auch "jemand") einen Zeitfaktor. Um
von der Winkelgeschwindigkeit auf einen Winkeländerung zu kommen, musst
du erstere mit der Zeitdauer zwischen jeweils zwei Sensorabfragen
multiplizieren, also:

        gz += (z_summe_off - z_off) * delta_t;

Bei konstanter Abfragerate kannst du delta_t auf einen festen Wert
setzen, andernfalls musst du delta_t für jede Abfrage mittels Timer
bestimmen.

Abgesehen davon sind deine Variablennamen teilweise sehr verwirrend.
Warum heißt bspw. die Winkelgeschwindigkeit in der while-Schleife
z_summe_off?

[Marco (Gast)]

Wieder einmal wünsche ich euch einen wunderschönen Guten Abend und 
bedanke mich bei yalau und dem netten Gast für die Tipps.

@yalu Kannst du mir mal einen kleinen Denkanstoß geben, wie ich die 
Zeitkonstante Variabel über einen Timer lösen kann. Habe mich schon die 
ganze Zeit gefragt, wie das mit der Zeitkonstante gehen soll. Hier in 
meinem Code ist ja der Ablauf periodisch, aber wenn dem nicht so wäre, 
hätte man ja nie einen genauen Messwert.

Dadurch das ich den Offset jetzt Subtrahiere und eine Zeitkonstante mit 
eingebaut habe, bleibt der Winkel relativ Konstant bei 0°. Wenn ich mir 
auch ganz viel Zeit nehme und den Sensor sehr sehr langsam um 90° Drehe, 
lande ich auch bei 90° und nach dem zurück drehen wieder bei 0°.

Wenn ich den Sensor aber schnell um 90° Drehe, werden die Werte 
verschluckt und der Winkel ändert sich vielleicht um 2-3°. Dachte es 
liegt daran, das ich jeden Wert am Rechner ausgebe, aber daran liegt es 
leider nicht.

Habe den Code auch mal aufgeräumt und ein wenig "lesbarer" gemacht :)

Was mich auch sehr wundert, meine Zeitkonstante ist 0,5625µs. Wenn ich 
mir aber ein Messdurchlauf anschaue, messe ich 2,2µs.

Vielleicht hat von euch jmd noch einen Tipp für mich. Drehe mich 
irgendwie im Kreis.

#include "mbed.h"
#include "stdio.h"

Serial pc(SERIAL_TX, SERIAL_RX);
SPI spi(PE_6,PE_5,PE_2);  //mosi,miso,sclk
DigitalOut ncs(PE_4);    //ssel

int main()
{
    spi.format(8,0);
    spi.frequency(1000000); 
    
    int16_t z_high, z_low, z_high_low;
    float z_winkel, z_off, z_high_low_summe;
    int16_t i;
    
    pc.printf("\n\r");
    
    /****************//******/
    /*Initialisieren*//*Gyro*/
    /****************//******/
    ncs = 0;
    spi.write(0x6B);     // Register 107
    spi.write(0x80);     //Reset // Standby off
    ncs = 1;               
    wait_ms(1000); 
    
        
    ncs=0;
    spi.write(0x1A);     //CONFIG  write // DLPF_CFG // Register 26
    spi.write(0x00);     //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz
    ncs = 1;                
    wait_ms(1);
    
    ncs=0;
    spi.write(0x1A);     //CONFIG  write//DLPF_CFG // Register 27
    spi.write(0x00);     //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz
    ncs = 1;                
    wait_ms(1);
    
    ncs=0;
    spi.write(0x1B);     //Gyro_CONFIG  write  
    spi.write(0x18);     //Max. Skalenwert//00=+250dps;08=+500dps;10=+1000dps;18=+2000dps
    ncs = 1;                
    wait_ms(1); 
    

    z_high = 0; 
    z_low = 0;
    z_high_low = 0;
    z_high_low_summe = 0;
    z_winkel = 0;
    z_off = 0;
    i = 0;

    /********//******/
    /*Offset*//*Gyro*/
    /********//******/ 
     
    for(i = 1;i <= 4000; i++)
    {
        ncs = 0;
        spi.write(0xc7);            //Z_OUT_H
        z_high = spi.write(0x0);
        ncs = 1;
        wait_ms(1); 
           
        ncs = 0;
        spi.write(0xc8);            //Z_OUT_L
        z_low = spi.write(0x0);
        ncs = 1;
        wait_ms(1); 
            
        z_high_low = z_low + z_high << 8;   
        z_high_low_summe += z_high_low;
    }
    pc.printf("\nOffset:\n\rZ = %2.2f\n\r", z_high_low_summe/4000.00);
    z_off = z_high_low_summe/4000.00;
    

    /********//******/
    /*Messen*//*Gyro*/
    /********//******/   
    z_high_low_summe = 0; 
    i = 0; 
    while(1)
    {  
        i++;    //Zähler fpr den Printf
        
        ncs = 0;
        spi.write(0xc7);            //Z_OUT_H
        z_high = spi.write(0x0);
        ncs = 1;
        wait_ms(1);
           
        ncs = 0;
        spi.write(0xc8);            //Z_OUT_L
        z_low = spi.write(0x0);
        ncs = 1;
        wait_ms(1);
                     
        z_high_low = z_low + z_high << 8;                   //Low und High Byte zusammenfügen
        z_high_low_summe = z_high_low - z_off;              //Offset vom Messwert subtrahieren
        z_winkel = z_winkel + (z_high_low_summe * 0.0000005625);   //Messwert multipliziert mit der Zeitdifferenz
        if (i == 1000)
        {
            pc.printf("z_Winkel: = %3.5f\t\r", z_winkel); 
            i = 0;
        }
    }
}

[Marco (Gast)]

Einen schönen Sonntag wünsche ich euch alle.

Habe jetzt meinen Code nocheinmal geändert und einen timer integriert.

So weit geht die Messung, jedoch nur bei langsamen Änderungen und was 
ich auch nicht verstehe, trotz des Timers muss ich noch mit einen festen 
Wert multiplizieren, um die Änderungen in Winkel zu haben.

Meine Einstellungen:
Full Scale Range: +-2000 °/s
Sensitivity Scale Factor: 16.4LSB/(º/s)

Bin auch so langsam mit meinem Latein am Ende und weiß nicht was ich 
noch machen kann -.- Ein Schleifendurchlauf dauert 119µs und die 
Abtastrate vom Sensor ist 8000Hz, also alle 125µs einmal Abtasten

    /********//******/
    /*Messen*//*Gyro*/
    /********//******/   
    z_high_low_summe = 0; 
    i = 0; 
    while(1)
    {  
        i++;    //Zähler fpr den Printf
        timer.start();
        
        ncs = 0;
        spi.write(0xc7);            //Z_OUT_H
        z_high = spi.write(0x0);
        ncs = 1;
        wait_us(1);
           
        ncs = 0;
        spi.write(0xc8);            //Z_OUT_L
        z_low = spi.write(0x0);
        ncs = 1;
        wait_us(70);
        zeit = timer.read_us();
        timer.reset();
        timer.start();
                     
        z_high_low = z_low + z_high << 8;                   //Low und High Byte zusammenfügen
        z_high_low_summe = z_high_low - z_off;              //Offset vom Messwert subtrahieren
        z_winkel = z_winkel + (z_high_low_summe * zeit * 0.0000000003);   //Messwert multipliziert mit der Zeitdifferenz
        if (i == 30000)
        {
            pc.printf("z_Winkel: = %3.5f\t%10luµs\r", z_winkel, zeit); 
            i = 0;
        }
    }
}

[Marco (Gast)]

Habe jetzt das Geheimnis gelüftet, es lag daran, das ich das high byte 
mit dem low byte addirt habe und nicht verodert