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”.