MPU6050 Sensor auslesen mit dem Netduino (Repost)

Fest verlötet auf meinem Shield.

Anfang des Jahres habe ich bereits darüber geschrieben und dazu auch ein Beispiel Projekt gepostet. Nun möchte ich diesen Artikel neu aufsetzen und etwas mehr auf einzelne Punkte eingehen. Zunächst nochmal der Hinweis, es handelt sich wieder um einen reinen Funktionalen Programm Code und hat daher keine Fehlerbehandlung.

Ein paar Informationen zu dem Sensor

Hersteller	InvenSens
Bezeichnung	MPU-6000 und MPU-6050
Sensor Achsen	6
Gyroskope Einstellungen	+-250°/s, +-500°/s, +-1000°/s, +-2000°/s
Beschleunigungseinstellungen	+-2g, +-4g, +-8g, +-16g
Verbindung	I²C (der 6000er hat zusätzlich SPI)

Kommen wir nun zum eigentlichen Thema. Ursprünglich gab es von Jeff Rowberg bereits eine fertige Library in C++. Leider ließ sich dieser Code nicht für den Netduino verwenden und daher strebte ich an, eine C# und .NET Micro Framework Variante zu schreiben. Zugegeben, alleine mit dem PDF Dokument hätte ich das nicht verstanden und war froh, die Funktionsweise aus der C++ Variante heraus zu lesen.

Fangen wir mit der Hauptklasse an. Der wichtigste Teil hierbei ist, mit welchen Einstellungen der Sensor Initialisiert wird.

public class MPU6050Sensor {     // I²C Klasse für die Verbindung zum Sensor     private I2CDevice _i2CDevice;     // Initialisiert den Sensor     public MPU6050Sensor()     {         // I²C Bus verbindung herstellen         // Die erste Hex Zahl stellt die Adresse vom Sensor und         // der Wert 100 steht für die Übertragung in kHz.         // Wahlweise kann hier auch 400kHz verwendet werden,         // setzt jedoch eine saubere Verbindung voraus,         // da Störungen das Ergebnis beeinflussen.         _i2CDevice = new I2CDevice(new I2CDevice.Configuration(0x68, 100));         // Sensor Initialisieren         // Sleep und Reset durchführen         // Der Hex Wert 0x6B steht für das Power Management 1         // Der zweite Hex Wert beinhaltet das 'Bit' für einen Reset.         StatusMessage(Write(new byte[] { 0x6B, 0x80 }));         Thread.Sleep(10);         // Sleep beenden und Clock einstelllen         // Der Hex Wert 0x6B steht für das Power Management 1         // Der zweite Hex Wert 0x00 legt die 'Clock Select'         // auf 'Internal 8MHz oscillator'         // Falls der Temperatur Sensor nicht ausgelesen werden soll,         // dann kann stattdessen der Wert 0x08 eingetragen werden.         StatusMessage(Write(new byte[] { 0x6B, 0x00 }));         // Konfiguration festlegen         // Diese Einstellungen aktiviert den Tief Pass Filter (DLPF) und         // wird z.B. verwendet, andere Vibrationen heraus zu filtern.         // Setting => Acc=5Hz, Delay=19.0ms, Gyro=5Hz, Delay=18.6ms, Fs=1kHz         StatusMessage(Write(new byte[] { 0x1A, 0x06 }));     }     // Sendet das Byte Array zum Modul    private int Write(byte[] buffer)     {         I2CDevice.I2CTransaction[] transactions = new I2CDevice.I2CTransaction[]         {             I2CDevice.CreateWriteTransaction(buffer)         };         return _i2CDevice.Execute(transactions, 1000);     }     // Liest mit den Byte Array die Daten vom Modul     private int Read(byte[] buffer)     {         I2CDevice.I2CTransaction[] transactions = new I2CDevice.I2CTransaction[]         {             I2CDevice.CreateReadTransaction(buffer)         };         return _i2CDevice.Execute(transactions, 1000);     }     // Status Nachrichten wiedergeben     private void StatusMessage(int result)     {         if (result == 0)         {             Debug.Print("Status: Fehler beim Senden oder Empfangen");         }         else         {             Debug.Print("Status: OK");         }     }     // Ruft den Sensor Daten ab.    public SensorData GetSensorData()     {         // Byte Array für den Lese Vorgang vorbereiten         byte[] buffer = new byte[14];         buffer[0] = 0x3B;         // Der Hex Wert stellt das erste Byte da         // für die Beschleunigungsachse X         Write(new byte[] { 0x3B });         // Byte Array übergeben zum beschreiben         Read(buffer);         // Ergebnis an das Sensor Objekt,         // in dem die Byte Werte umgewandelt werden         return new SensorData(buffer);     } }

Nun das war vielleicht etwas viel Green Code, dennoch hoffe ich, das die Beschreibungen etwas Aufschluss geben, was diese Byte Werte alle sollen. Weniger aufwendig und kürzer ist das SensorData Objekt, das die Byte Daten in brauchbare Werte umwandelt.

public class SensorData {     public uint Acceleration_X = 0;     public uint Acceleration_Y = 0;     public uint Acceleration_Z = 0;     public uint Temperatur = 0;     public uint Gyroscope_X = 0;     public uint Gyroscope_Y = 0;     public uint Gyroscope_Z = 0;     public SensorData(byte[] buffer)     {         // Ergebnis für den Beschleunigungssensors zusammenlegen durch Bitshifting         Acceleration_X = (((uint)buffer[0]) << 8) | buffer[1];         Acceleration_Y = (((uint)buffer[2]) << 8) | buffer[3];         Acceleration_Z = (((uint)buffer[4]) << 8) | buffer[5];         // Ergebnis für Temperatur         Temperatur = (((uint)buffer[6]) << 8) | buffer[7];         // Ergebnis für den Gyroskopsensors zusammenlegen durch Bitshifting         Gyroscope_X = (((uint)buffer[8]) << 8) | buffer[9];         Gyroscope_Y = (((uint)buffer[10]) << 8) | buffer[11];         Gyroscope_Z = (((uint)buffer[12]) << 8) | buffer[13];     } }

Nun fehlt noch der Einsatz in der ‘Main’ Methode, dass hier die Klasse für den Sensor initialisiert, den Abruf in das SensorData Objekt kopiert und schließlich über Debug.Print die Daten anzeigt..

public static void Main() {     MPU6050Sensor mpu = new MPU6050Sensor();     SensorData data = mpu.GetSensorData();     while (true)     {         data = mpu.GetSensorData();         Debug.Print(             "Acc X: " + data.Acceleration_X + " " +             "Acc Y: " + data.Acceleration_Y + " " +             "Acc Z: " + data.Acceleration_Z + " " +             "Gyro X: " + data.Gyroscope_X + " " +             "Gyro Y: " + data.Gyroscope_Y + " " +             "Gyro X: " + data.Gyroscope_Z + " " +             "Temperatur: " + data.Temperatur);     } }

Ist dieser Teil eingeben, so kann es dann auf dem Netduino weiter gehen. Hierbei kann ich den Tipp geben, auf die Verkabelung zu achten. Zwar passiert nichts, wenn ihr etwas nicht richtig angeschlossen habt, jedoch wenn ein Wackelkontakt vorliegt oder das Kabel zu lang ist für die I²C Verbindung, kann es zu erheblichen Störungen kommen.

Die Verbindung von Netduino zum Sensor:

• 3,3V >> VCC
• GND >> GND
• AnalogIn 4 >> SDA
• AnalogIn 5 >> SCL

Ist der Anschluss erfolgt, kann das Programm auf den Netduino geschrieben werden. Sobald der Sensor Initialisiert wurde und der Programmcode in die Schleife geht, wird das Ergebnis über Debug.Print() ausgegeben.

In Visual Studio 2012 wird über die ‘Ausgabe’ oder im Englischen ‘Output’ ausgegeben.

Falls Fragen bestehen kann gerne ein Kommentar abgegeben werden oder einen Eintrag im Netduino Forum schreiben. Dort bin ich unter dem Namen Heroduino zu finden.

Natürlich wie immer die Solution für das .NET Micro Framework Version 4.2

• Download Example MPU6050

DLPF einstellen für den MPU6050

Kleiner Versuchsaufbau mit Sensor und Brushless Motor

Auf einem Quadrocopter sorgen die Motoren für viel Vibrationen, die sich ohne einen Filter auf den Sensor negativ auswirken. Über ein Verlaufsdiagramm über die Beschleunigung und die Winkelgeschwindigkeit kann man deutlich hoch frequentierte Abweichungen sichtbar machen. In einem meiner ersten Tests hatte ich die Funktion DLPF (Digitaler Low-Pass Filter) vom Sensor nicht eingeschaltet und wunderte mich zunächst, warum die Lage nicht reguliert wurde. Also erweiterte ich mein Programm zur Analyse und sah dann das Problem.

Das nächste Bild zeigt den Sensor stillliegend mit laufendem Motor. Die schwarze Linie zeigt die Rohdaten an, die grüne Linie zeigt das Ergebnis nach einer Glättung an.

Die Vibrationen vom Motor erschweren es, die eigentlich Lage festzustellen, die der Sensor wirklich hat.

Zuvor kannte ich die Lösung von analogen Tiefpassfiltern. In der Digitaltechnik sieht das etwas anders aus. Es ist ein zeitkritisches Thema, wenn dort der Filter ausgeführt wird. Zu empfehlen ist, einen Microcontroller alleine zu verwenden, wenn der Sensor keinen eigenen hat.

Zum Glück gibt es das DLPF im MPU6050 Sensor, der 7 verschiedene Grenzfrequenzen zur Verfügung stellt und somit die hoch frequentierten Störungen ausfiltern kann.

Nachdem ich nicht ganz so müde an die Sache rangegangen bin, verstand ich auch die Einstellung mit dem DLPF (Digitaler Low-Pass Filter) und legte die Einstellungen in der Initialisierung fest. In meinem Beispiel Projekt vom Januar reicht es, nur diese eine Codezeile hinzuzufügen (besser ist, wenn ihr die neue Solution herunter ladet).

// Konfiguration festlegen // Einstellung => Acc=5Hz, Delay=19.0ms, Gyro=5Hz, Delay=18.6ms, Fs=1kHz StatusMessage(Write(new byte[] { 0x1A, 0x06 }));

Wie schon im Kommentar zu sehen ist. Lässt der Sensor nur noch Bewegungen bis 5Hz zu. Alles was darüber steht, wird ausgefiltert.

Der neue Test mit dem DLPF zeigt ein deutlich besseres Ergebnis. Egal wie weit ich nun den Motor ansteuere, die Messdaten zeigen weiter die ruhige Lage des Sensors.

Nach der Einstellung des DLPF ist von der Motorenvibration (fast) nichts mehr zu sehen.

Je nach Anwendung könnte die Grenzfrequenz von 5Hz natürlich auch etwas zu niedrig sein. Betrachtet man den Quellcode vom WiiCopter, so ist zu sehen, dass die Grenzfrequenz bei 260Hz liegt.

Wie bereits im Codeausschnitt zu sehen ist, werden zwei Bytes in einem Array übergeben. Das Byte ‘0x1A’ steht für den Aufruf der Konfiguration. Das zweite Byte legt die gewünschte Einstellung fest, die ich in der folgenden Tabelle aufliste:

Byte	Inhaltliche Einstellung
0x00	Accelrometer => 260Hz, Gyroscope => 256Hz
0x01	Accelrometer => 184Hz, Gyroscope => 188Hz
0x02	Accelrometer => 94Hz, Gyroscope => 98Hz
0x03	Accelrometer => 44Hz, Gyroscope => 42Hz
0x04	Accelrometer => 21Hz, Gyroscope => 20Hz
0x05	Accelrometer => 10Hz, Gyroscope => 10Hz
0x06	Accelrometer => 5Hz, Gyroscope => 5Hz

Beispiel Solution für Netduino: ExampleMPU6050Sensor.zip