Kreisgeschwindigkeit aus dem Sensor umrechnen (Die Zweite)

Von mir entwickeltes Programm zum darstellen eines künstlichen Horizontes und ein Würfel, das die Bewegungen des Sensors wieder gibt.

Im letzten Post zeigte ich eine relativ ungenaue Lösung für das Umrechnen der Winkellage. Grund hierfür ist, dass der Zeitabstand nicht verwendet wird. Als Provisorium reicht es allerdings aus.

Nun, welche Formel wird stattdessen benötigt?

Kreisgeschwindigkeit = Drehwinkel / Zeitspanne

steht für die Winkelgeschwindigkeit

steht für den Drehwinkel

steht für die vergangene Zeit

Im nächsten Schritt muss ich die Formel umstellen, um später an das Ergebnis des Drehwinkels heran zu kommen. Der Sensor gibt pro Abfrage die Winkelgeschwindigkeit, und die Zeit wird zwischen der aktuellen Abfrage und der letzten Abfrage bestimmt.

Die verwendeten Werte nehme ich wie zuletzt aus dem Sensor MPU6050 mit den entsprechenden Einstellungen. Der Wert 32768 ist der Maximale Einheiten Wert der in eine Richtung gemessen werden kann. Die vergangene Zeit wird durch 1000ms dividiert, weil die Formel eigentlich für Sekunden ausgelegt ist. Die 2000.0 kommt aus der Einstellung für die Gyroskop Empfindlichkeit, also 2000°/s.

Daraus kann nun der Programmcode gebildet werden.

double winkel = ((((measurement - _Offset) / 32768.0) *                                   ((double)timeelapsed / 1000.0))) * 2000.0;

measurement = Messergebnis _Offset = Abweichungswert das den Messwert an die eigentliche Lage bringt timeelapsed = vergangene Zeit zwischen der Abfrage und der letzten

Die Aufstellung ist für den Netduino relativ lang und sollte daher gekürzt werden. Am besten geht das auf einem Blatt Papier.

Überflüssiges kürzen

Somit sieht dann der Programmcode schon etwas schlanker aus.

double winkel = ((measurement - _Offset) * (double)timeelapsed) / 16384.0;

Der errechnete Wert zeigt den zurückgelegten Winkel. Damit daraus eine Lagebestimmung wird, muss daraus eine Integralrechnung werden, sowie bereits in meinem letzten Blogeintrag erwähnt. Damit sieht nun die Klasse folgendermaßen aus:

public class GyroscopeAxis {     /// Derzeitige Winkellage     private double _Angle = 0;     /// Ruft ab, ob der Offset gesetzt wurde oder legt diesen fest.     private bool _IsOffset_Done = false;     /// Ruft den Offset ab oder legt diesen fest.     /// Wird benötigt, um die Abweichung runter zurechnen.     private double _Offset = 0;     /// Nimmt den aktuellen Beschleunigungswert und summiert den auf den Winkelwert     public double UpdateAngle(double measurement, ref int timeelapsed)     {          // Offsetwert festlegen, wenn nicht gesetzt.         if (!_IsOffset_Done)         {             _Offset = measurement;             _IsOffset_Done = true;         }        // Aktuellen Wert summieren und den Offset Wert Abziehen         _Angle -= ((measurement - _Offset) * (double)timeelapsed) / 16384.0;         return _Angle;     } }

Die Klasse für das Bestimmen der Winkellage

Anders als in meinem vorigen Blog Post habe ich die Abweichung (Offset) mit eingebracht. Dieser Wert wird beim Kalibrieren ermittelt, um die waagerechte Haltung anzugleichen. Was allerdings noch bleibt und auch nicht zu verhindern ist, sind weiterhin die Messfehler, die nur durch das Komplementieren mit der Beschleunigungsdaten gefiltert werden können.

Kreisgeschwindigkeit aus dem Sensor umrechnen

Von mir entwickeltes Programm zum darstellen eines künstlichen Horizontes.

Neben Beschleunigung existiert die Kreisgeschwindigkeit und kann durch ein Gyroskop gemessen werden. Um nun daraus einen Winkel zu ermitteln, ist eine einfache Rechnung erforderlich.

(Aktueller Winkel) = (Aktueller Winkel) + (Messwert)

Sieht also nicht sonderlich wild aus und genauso wenig schwierig ist dies in ein Programm zu schreiben.

private double _Angle = 0; public double UpdateAngle(double measurement) {     _Angle += measurement;     return _Angle; }

Von Gyroskop zu Gyroskop sind die Ergebnisse unterschiedlich. Wie immer nehme ich den MPU6050 Sensor, der einen Messbereich von 0 bis 65535 wiedergibt. Um auch hier negative Werte zu erhalten, ist es nötigt, die Werte um 32768 zu verschieben. Sowie ich das im Blog Post mit dem Beschleunigungssensor Ergebnis erledigt habe. Zudem muss noch der Wert skaliert werden, der davon abhängig ist, auf welche Messgeschwindigkeit eingestellt wurde. Hierzu habe ich eine Tabelle mit Werten zusammengestellt.

250°/s	(Messwert) / 8192 (oder 10000)
500°/s	(Messwert) / 4096
1000°/s	(Messwert) / 2048
2000°/s	(Messwert) / 1024
	Leider kann es hier Abweichungen zu meinem Beispiel geben. Hier muss man im nachhinein schauen ob das passt.

Mit dieser Information kann nun die Methode ergänzt werden.

public double UpdateAngle(double measurement) {     measurement = measurement / 1024;     _Angle += measurement;     return _Angle; }

Indirekt war das nun der zweite Teil für den künstlichen Horizont. Damit auch dieser Code vollständig funktioniert werden zur Korrektur die Ergebnisse aus dem Beschleunigungssensor benötigt. Der Grund hierfür ist, dass Messfehler entstehen können, die dazu führen, dass sich die Lage nach und nach verschiebt. Das Stichwort an dieser Stelle heißt “Komplementieren”.

MPU6050 Sensor auslesen mit dem Netduino

Netduino mit Shield - Aufsatz, für Sensor und Reglern

Für mein Quadrocopter Projekt habe ich vor einigen Monaten einen zusätzlichen Sensor benötigt, da ich mit einem Beschleunigungssensor allein Schwierigkeiten hätte, das Gleichgewicht und die Lage festzustellen. Um dies etwas kompakter zu gestalten, fand ich im Internet das MPU6050 Sensor Modul, das einen Beschleunigungssensor und Gyroskopsensor auf einem Chip hat. Über Ebay erhält man das Modul zu einem günstigen Preis, man muss allerdings mit einer gewissen Wartezeit rechnen, so ca.. 3-4 Wochen.

MPU6050 (Die Platine selbst, scheint für verschieden Sensor Typen gedacht zu sein, wegen der Aufschrift MPU-x050)

Im Arduino Umfeld scheint der Sensor seine Verbreitung gefunden zu haben. Das dürfte auch an der fertigen Library liegen, womit sich in nur wenigen Codezeilen der Sensor für seine eigenen Projekte einbinden lässt . Eine bereitgestellte Lösung für Netduino gab es dafür nicht, was mich um so mehr dazu bewegt hat, hierfür eine Lösung zu finden.

Der Sensor verbunden mit dem Arduino Nano 3.0

Mein Ansatz war, den verfügbaren Code in C++ nach C# und .Net Micro Framework zu portieren. Das hieß viele Stunden Quellcode und Dokumentation zum Sensor lesen. Die Dokumentation befasst sich überwiegend mit den Befehlen des Mikroprozessors vom Sensor, was aber anhand der Beschreibung nur schwer nachvollziehbar ist. Überwiegend hat mir der Quellcode selbst am weitesten geholfen, um die Prozessreihenfolge zu verstehen. Am Anfang funktionierte der Code nur teilweise und ich bekam einfach keine brauchbaren Daten. Mehrmals kontrollierte und überarbeite ich das Initialisieren des Sensors über die Monate hinweg. Dazu muss ich hinzufügen, dass ich nur wenig Zeit hatte.

Unregelmäßige Werte vom Sensor. (Ausgabe Wert wird hier verkürzt dargestellt zwischen 0 bis 100)

Bis gestern dachte ich, dass ich mit dem Initialisieren den falschen Byte Wert verwendet habe. Es stellte sich heraus, dass ich bereits beim Einbinden des Sensors über den I²C Bus richtig gelegen hatte. Eine fehlende Codezeile, die ich an einer anderen Stelle gesetzt habe, wirkte Wunder. Also wichtiger Hinweis: “Bevor die Werte gelesen werden können, muss der erste Bytewert zunächst an den Sensor geschrieben werden.” Anschließend bekam ich endlich konstante und brauchbare Daten.

Stabile Werte, die sich erst verändern, wenn ich den Sensor bewege.

Nun käme normalweise der Punkt, wo ich nun endlich den Quellcode posten würde. Stattdessen hänge ich einen link an, und biete dieses als Download an, da sich der Blogeintrag zu sehr in die länge ziehen würde.
Beispiel Solution für den Sensor MPU6050

Hinweis zum Beispiel Quellcode:

• Der Quellcode dient als Beispiel und ist daher keine Lösung
• Soweit ich konnte, habe ich überall Kommentare eingetragen
• Ich bin für Fragen und (sinnvolle) Kritik offen

Nachtrag (07.07.2013):

Zu dem Thema habe ich eine neuen Blog Post veröffentlicht

Vorerst mein letzter Sensor für mein Projekt Quadrocopter

Der Sensor GY-52

Es hat wieder einige Wochen gedauert, bis meine Bestellung aus China kam. Dafür hat es sich gelohnt, einige zig Euros zu sparen. Und wie schon gesagt, ich habe Zeit.
Zunächst sehe ich zu, dass ich die Daten einlesen kann, und wie immer in C und C# werde ich Beispiele dazu posten.

Nachtrag:
Nachdem ich auch die Dokumentation zu dem Sensor gefunden habe, musste ich feststellen, dass es nicht gerade so einfach wird, die Daten auszulesen. Derzeitig bekomme ich zwar eine Verbindung, jedoch keine Daten.
Bis ich mit der Doku durch bin, werde ich erstmal weiter an der Windows Phone 7 App und dem Programmcode für den Controller arbeiten.