Okay ich hab das Auslesen hinbekommen, hier der Code:

#define F_CPU 32000000UL

#include <inttypes.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>
#include <stdint.h>

#define TwiStart TWIC_MASTER_ADDR
#define TwiStop TWIC_MASTER_CTRLC = TWI_MASTER_CMD_STOP_gc
#define TwiData TWIC_MASTER_DATA
#define TwiStatusW while(!(TWIC.MASTER.STATUS & TWI_MASTER_WIF_bm))
#define TwiStatusR while(!(TWIC.MASTER.STATUS & TWI_MASTER_RIF_bm))

void TWI_MasterInit()
{
TWIC.CTRL = 0x00; //Normal TWI mode
TWIC.MASTER.BAUD = 75;//155--> 100 KHz at 32MHz clock; 75--> 200 KHz at 32MHz clock
TWIC.MASTER.CTRLA = TWI_MASTER_ENABLE_bm;
TWI_MASTER_INTLVL_HI_gc;
TWI_MASTER_RIEN_bm;
TWI_MASTER_WIEN_bm;
TWIC.MASTER.CTRLB = TWI_MASTER_TIMEOUT_DISABLED_gc; //Timeout disabled
TWIC.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; //Set bus state idle
}


void WMP_INIT()
{
TwiStart = 0xA6;
TwiStatusW;

TwiData = 0xFE;
TwiStatusW;

TwiData = 0x04;
TwiStatusW;

TwiStop;
}


void WMP_LESEN() //aktuelle Messwerte auslesen
{
uint8_t wmpbyte[6];
TwiStart = 0xA4;
TwiStatusW;

TwiData = 0x00;
TwiStatusW;

TwiStop;

TwiStart = 0xA5;
TwiStatusR;

// Read data + send ACK
for(uint8_t i = 0; i < 6 ; i++)
{
while (!(TWIC_MASTER_STATUS & TWI_MASTER_RIF_bm));
wmpbyte[i] = TwiData;
TWIC_MASTER_CTRLC = TWI_MASTER_CMD_RECVTRANS_gc;
}
TWIC_MASTER_CTRLC = 0x06;//NACK, indicating that we are done reading

YawSpeed = wmpbyte[3] << 6; YawSpeed = YawSpeed >> 7; //Slow/Fast Bit
PitchSpeed = wmpbyte[3] << 7; PitchSpeed = PitchSpeed >> 7; //Slow/Fast Bit
RollSpeed = wmpbyte[4] << 6; RollSpeed = RollSpeed >> 7; //Slow/Fast Bit

for (uint8_t i = 3; i <= 5; i++) //Byte 3-5 die Bits 0 und 1 entfernen
{
wmpbyte[i] = wmpbyte[i] >> 2;
}

Yaw = ((wmpbyte[3] << 8 | wmpbyte[0]); //Zusammenfügen --> 14Bit
Roll = ((wmpbyte[4] << 8 | wmpbyte[1]); //Zusammenfügen --> 14Bit
Pitch = ((wmpbyte[5] << 8 | wmpbyte[2]); //Zusammenfügen --> 14Bit
}


void WMP_Kalibrierung() //60 Messungen durchführen
{
for (uint8_t i = 0; i < 10; i++) //10 Messungen durchführen da die ersten Fehlerhaft sind
{
WMP_LESEN();
}

for (uint8_t i = 1; i <= 50; i++) //Kalibrierungsmessungen
{
WMP_LESEN();
YawOffset = YawOffset + Yaw;
RollOffset = RollOffset + Roll;
PitchOffset = PitchOffset + Pitch;
}
YawOffset = YawOffset / 50;
RollOffset = RollOffset / 50;
PitchOffset = PitchOffset / 50;
}

Ich hoffe mit dem Code kann der ein oder andere etwas anfangen .

Nun ist mein neues Problem die Winkelberechnung. Mein bisherigen Versuche funktionieren, sind nur leider sehr ungenau.
Zuerst Initialisiere ich die WiiMotionPlus und führe die Kalibrierungsschleife aus.
Der Wert für "RollOffset" beträgt ca 9312.
Dieser wird beim nächsten Auslesen benötigt: Rolldifferenz = RollMesswert - RollOffset
Um auf einen Winkel zu kommen muss ich doch lediglich integrieren? --> Winkel = Winkel + Roll

Nun hat der Gyro leider einen kleinen Drift von (Messwert) ~9300 - 9322. Ich hab gelesen das dieser Drift auch ohne Beschleunigungssensor
kompensiert werden kann.

Meine bisherigen Versuche sahen folgendermaßen aus:
Roll = Roll - RollOffset;
Roll = Roll / 20;
RollWinkel = RollWinkel + Roll;
Durch die Teilung der Differenz ist das Rauschen weg, damit leider auch etwas Genauigkeit.

Zweiter Versuch:
Roll = Roll - RollOffset;

if (Roll > 19)
{
RollWinkel = RollWinkel + Roll;

}

if (Roll < -14)
{
RollWinkel = RollWinkel + Roll;
}
Die Werte 19 und -14 habe ich langsam erhöht und getestet ob sich in Ruhelage der Winkel verändert.
Zu meinem erstaunen sind sie sogar unterschiedlich. Diese Methode hatte mir bisher die besten Ergebnisse geliefert.

Hat jemand eine Idee zur Rechnung? Wie bekomme ich den Drift weg und einen genauen Winkel?
Auf http://wiibrew.org/wiki/Wiimote/Exte...ii_Motion_Plus gibt es unter Data Format einige Infos zum auslesen und der Winkelberechnung!

Vielen dank.