WiiMotionPlus TWI-Kommunikation mit einem xmega

[D35troy3r]

Hallo,
wie der Titel schon sagt versuche ich eine Verbindung zwischen einem xmega und einer Wiimotionplus herzustellen.
In Bascom ist es mir gelungen! Doch wird es Zeit auf C umzusteigen, allein schon weil es keinen Code in Bascom für ein RFM70 Funkmodul gab ;).
Nun ja, ihr seht meine ersten Versuche. Der Ablauf des Programms ist so wie zu sehen von oben nach unten.

Die Variable WMP gebe ich mir auf einem LCD aus. Leider ist sie bisher immer leer. Ich weiß weder ob die Initialisation richtig verläuft oder ob nur das Auslesen Fehler birgt.
In Bascom sah das Auslesen folgendermaßen aus:

I2cstart #2
I2cwbyte &HA4 , #2 'sends memory address
I2cwbyte &H00 , #2 'sends zero before receiving
I2cstop #2
I2creceive &HA4 , Buffer(1) , 0 , 6 , #2 'receive 6 bytes

Hab ich eventuell was mit den Adressen falsch gemacht? Denn dort ist die Adresse immer A4...A4 fürs schreiben? Und beim Auslesen ändert sie sich doch auf A5?

Aller Anfang mit C ist schwer....leider im Moment zu schwer ;). Ich danke allen Antwortenden :)


C-Code:


void TWI_MasterInit()
{
TWIC.CTRL = 0x00; //Normal TWI mode
TWIC.MASTER.BAUD = 0x9B;//100 KHz at 32MHz clock
TWIC_MASTER_CTRLA = TWI_MASTER_ENABLE_bm;
TWIC.MASTER.CTRLB = TWI_MASTER_TIMEOUT_DISABLED_gc; //Timeout disabled
TWI_MASTER_INTLVL_HI_gc;
TWI_MASTER_RIEN_bm;
TWI_MASTER_WIEN_bm;
TWIC_MASTER_STATUS = TWI_MASTER_BUSSTATE_IDLE_gc;
}


void WMP_INIT()
{
TWIC_MASTER_ADDR = 0xA6; //bisherige i2c Schreibadresse
TWIC_MASTER_DATA = 0xFE; //Registeradresse
TWIC_MASTER_DATA = 0x04; //i2c Schreibadresse in 0xA4 geändert
TWIC_MASTER_CTRLC = TWI_MASTER_CMD_STOP_gc;
}


void WMP_LESEN()
{
TWIC_MASTER_ADDR = 0xA4; //i2c Schreibadresse
TWIC_MASTER_DATA = 0x00; //schreibe nullen
TWIC_MASTER_CTRLC = TWI_MASTER_CMD_STOP_gc;

//TWIC.MASTER.CTRLC = TWI_MASTER_CMD_REPSTART_gc;
TWIC_MASTER_ADDR = 0xA5; //i2c Leseadresse
WMP = TWIC_MASTER_DATA;
TWIC_MASTER_CTRLC = TWI_MASTER_CMD_STOP_gc;
}

[D35troy3r]

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/Extension_Controllers/Wii_Motion_Plus gibt es unter Data Format einige Infos zum auslesen und der Winkelberechnung!

Vielen dank.

[D35troy3r]

Hat denn niemand eine Idee?
Okay, weiß denn wenigstens jemand eine Formel wie ich den Gyro mit einem Beschleunigungssensor ausgleichen kann? Ich durchsuche etliche Seiten und finde nichts brauchbares :(.
Mein Beschleunigungssensor ist von Pollin: http://www.pollin.de/shop/dt/NjU4OTgxOTk-/Bausaetze_Module/Module/3_Achsen_Beschleunigungssensor_Modul.html
300mV/g

Ich hab leider noch keine richtige Vorstellung wie das ganze kombiniert wird.

[Che Guevara]

Hi,

also zur Datenfusionierung gibts mehrere Ansätze, unteranderem einen Komplementärfilter (einfach und gut) und einen Kalmanfilter (schwierig und besser(?)). Ich selbst verwende einen Komplementärfilter. Ein Ansatz wäre:

a = 0.99
winkel = (a * (winkel + gyro * dt) + (1-a)*(acc_winkel)

Wobei a sozusagen die Grenzfrequenz der beiden Filter (Hochpass & Tiefpass) angibt, diese Variable musst du evtl. verändern, bis der Winkel stabil ist.
gyro ist der Wert vom Gyro (wer hätte das gedacht!? :D ) und acc_winkel ist der Winkel berechnet aus den Daten des ACCs. Bis ca. 30° kannst du aber auch eine Kleinwinkelnäherung verwenden. dt ist die Zeit, die seit der letzten Berechnung vergangen ist, also 1 / f, wobei f die Frequenz deines Programms ist.

Gruß
Chris

[D35troy3r]

Hey super danke! :D
Jetzt muss ich mir nur eine Formel zur Berechnung des Winkels vom ACC suchen :).
Ich berichte wieder wenn es funktioniert.

[Che Guevara]

Hi,

den Winkel kannst du am besten mit der ATN(2)-Funktion berechnen, das dauert allerdings auch ne Weile ;)

Gruß
Chris

[D35troy3r]

Habe gerade ein problem mit dem Endergebnis. Es rauscht extrem obwohl die Messwerte nicht rauschen?!

ACC_X = ACC_X - 3520; <<< Messwert - Offset
ACC_X = ACC_X / 4; <<<Differenz "entrauschen"
ACC_Y = ACC_Y - 3520;
ACC_Y = ACC_Y / 4;

ACC_X = ACC_X * 0.01564; <<< Differenz * Beschleunigungsfaktor = Beschleunigung
ACC_Y = ACC_Y * 0.01564;
float rollAcc = atan2f((float)ACC_X, (float)ACC_Y) * 180 / M_PI; <<< Umwandlung in einen Winkel

Die Werte ACC_X und ACC_Y geben im Stillstand "0" aus. Und trotzdem schwankt der winkel rollAcc von -13 bis sogar 60.
Bin noch nicht so der Profi was C angeht...fehlt mir vllt eine Kleinigkeit?!

P.s: alle Variablen sind floats

Edit2:
Hab eben mal folgendes aus einem Forum probiert:

Wenn Du einen z.B. einen +/-1.7G Sensor hast, der in Mittellage 2.5V ausgibt, bei -1.7G=0V und bei +1.7G = 5V dann hättest Du bei +1G=(2.5V/1.7)+2.5V = 3.97V und bei -1G=(2.5V/1.7)+2.5V = 1.03V
+/-1G hast Du dann, wenn Du den Sensor genau 90° seitlich kippst. Der Verlauf der Spannung folgt dabei der Sinus-Funktion im Bereich 0..90°. Im Bereich von ca. +/-15° ist der Verlauf der Sinus-Kurve recht linear, da kann man - je nach Anforderung an die Genauigkeit - fast ohne Kompensation der Ausgangsspannung leben. Darüber hinaus KANN man die Ausgangsspannung entsprechend kompensieren. Wenn man das z.B. mit einem Mikrocontroller macht, dann muss man eingangsseitig schon sehr fein auflösen (z.B. mit 14 bit), um ausgangsseitig noch eine annehmbare Auslösung (z.B. 8..10 bit) erzielen zu können.

Das funktioniert gerade sehr gut :).