Winkelgeschwindigkeit Integrieren

[Tomatenbaum]

Hallo

Ich versuche eine simple Integration von Winkelgeschwindigkeit eines Gyros um auf die Winkel zu kommen.
Mir ist klar das das Integrieren auch das aufsummieren von Fehlern bedeutet und der Winkel innert einigen Sekunden nicht mehr stimmt.
Mir geht es hier aber um das verstehen, später kommt dann eventuell das verrechnen von einem ACC.

Ich habe nun viele Tage Gesucht und die Beispiele sehen immer gleich aus:

angle = angle + gyro * dt;

Ich denke angle ist Winkel, gyro ist die Winkelgeschwindigkeit, dt die Zeit die ein Loop Durchgang benötigt.

nur bin ich vermutlich zu Blind um die Rohdaten aufzubereiten.

Die Hardware ist ein simpler Arduino uno mit dem 328.
Breakout mit MPU6050 = http://www.sparkfun.com/products/11028
Mit I2C ausgelesen lese ich nun folgende max -min werte = -32767 bis 32768.

Wie muss ich nun die Werte aufarbeiten?

Angefangen habe ich damit den Offset abzuziehen:
Im Setup mache ich dies:

 for (int i=0;i<500;i++) //mittelung von nullpunkt
    {
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      gz_null = gz_null+gz;
    }
    gz_null = gz_null/500;

Danach versuche ich das Integrieren:

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

float gz_null=0;
float winkel=0;


void setup() {
   
    Wire.begin();
    Serial.begin(38400);
    accelgyro.initialize();
    
    
    for (int i=0;i<500;i++) //mittelung von nullpunkt
    {
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      gz_null = gz_null+gz;
    }
    gz_null = gz_null/500;
    
    
}

void loop() {
  
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //MPU6050 auslesen
    gz = gz - gz_null; //offset abziehen
    winkel=winkel+gz*2.8; //Winkel Integrieren, winkel + winkel * 2.8ms (dauer zwischen 2 loop durchgängen.
    
    Serial.println(winkel); //Winkel ausgeben
    
}

Jetzt habe ich einen Wert, das sich schön ändert wenn ich den Gyro langsam um die Achse drehe, je nach Richtung nimmt dieser Wert zu oder ab.
Aber wenn ich den Gyro schneller drehe, verschluckt sich das und die Werte ändern viel zu wenig.

Habe ich nun kompletten Murks Programmiert?
Muss ich die Gyro Rohdaten ( -32767 bis 32768 ) zuerst aufbereiten?
Wie bekomme ich hier nun den Winkel in °?

Gruss Tomatenbaum