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.