Donnerstag, 18. Juli 2013

Kreisgeschwindigkeit aus dem Sensor umrechnen

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

Sonntag, 7. Juli 2013

MPU6050 Sensor auslesen mit dem Netduino (Repost)

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

Netduino_MPU6050

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.

image
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

Sonntag, 23. Juni 2013

Zeitkritische Momente mit dem Netduino

Quadrocopter_48
USB TTL UART, Netduino mit Shield und Schaltnetzteil für Tests.

Bei einigen Anwendungen kommt es vor, dass eine Iteration möglichst wenig Zeit benötigen soll. Das bedeutet, dass man sich mit Code Optimierung beschäftigen muss, in dem man zumeist die gewohnte Art zu Programmieren überdenken muss. Grundsätzlich sollte man das nicht tun, wenn man Anfänger ist, und dann auch nicht wenn man professioneller Entwickler ist. Es sei denn, die Anforderung erfordert diese Optimierung. In der Entwicklung für meinen Quadrocopter stieß ich auf dieses Problem, dass eine Iteration viel zu lange dauert. Zwar sauber geschrieben, jedoch nicht Performance-optimiert, kam ich auf ca. 40ms bis 45ms pro Iteration. Das ergibt pro Sekunde 22 bis 25 Durchläufe. Nach den Optimierungen kam ich auf Iterationslaufzeiten von 11ms bis 22ms. Diese weite Zeitspanne entstand durch dynamische Verarbeitung von Byte Werten beim Senden und Empfangen, was jedoch ein anderes Thema abbilden würde. Zunächst benötigen wir eine Methode, um herauszufinden, wie lange ein Vorgang an Zeit benötigt.

public class Stopwatch
{
    private static long _Ticks_Start = 0;
    private static long _TicksPerMillisecound = System.TimeSpan.TicksPerMillisecond;

    public static long GetElapsedMillisecounds()
    {
         long result = (Microsoft.SPOT.Hardware.Utility.GetMachineTime().Ticks
            - _Ticks_Start) / _TicksPerMillisecound;

         _Ticks_Start = Microsoft.SPOT.Hardware.Utility.GetMachineTime().Ticks;

        return result;
    }

    public static long GetElapsedMicrosecounds()
    {
         long result = (Microsoft.SPOT.Hardware.Utility.GetMachineTime().Ticks
            - _Ticks_Start) / (_TicksPerMillisecound / 1000);

         _Ticks_Start = Microsoft.SPOT.Hardware.Utility.GetMachineTime().Ticks;

         return result;
    }
}

Die Klasse habe ich aus dem Netduino Forum und ist für dieses Beispiel angepasst.

Kommen wir zu einigen Beispielen, in dem einfach einer Integer-variablen ein Wert zugewiesen wird der dann durch einen anderen dividiert wird. Nach 100 Durchläufen werden die Durchschnittszeiten der 6 Beispieel ausgegeben. Der Grund ist, dass das Ergebnis nicht konstant bleibt und durch weitere Einflüsse beeinträchtigt ist.

public class Program
{
    public static void Main()
    {
        long timeResult = 0;
        int result = 0;

        long countIteration = 1;
        long avarageExample1 = 0;
        long avarageExample2 = 0;
        long avarageExample3 = 0;
        long avarageExample4 = 0;
        long avarageExample5 = 0;
        long avarageExample6 = 0;

        while (true)
        {

           // Beispiel mit einer Devisions Rechnung in Millisekunden
            Stopwatch.GetElapsedMillisecounds();

            result = 12345;
            result = result / 67890;

            timeResult = Stopwatch.GetElapsedMillisecounds();
            Debug.Print("Beispiel 1: " + timeResult +
                " Millisekunde, Freier Speicher: " + Debug.GC(true) + "kb");
            avarageExample1 += timeResult;

           // Beispiel mit einer Devisions Rechnung in Mikrosekunden
            Stopwatch.GetElapsedMillisecounds();

            result = 12345;
            result = result / 67890;

            timeResult = Stopwatch.GetElapsedMicrosecounds();
            Debug.Print("Beispiel 2: " + timeResult +
                " Mikrosekunden, Freier Speicher: " + Debug.GC(true) + "kb");
            avarageExample2 += timeResult;

            // Beispiel mit einer Devisions Rechnung
            // über eine Methode und den Integer kopieren.

            Stopwatch.GetElapsedMillisecounds();

            result = 12345;
            result = GetDivided(result);

            timeResult = Stopwatch.GetElapsedMicrosecounds();
            Debug.Print("Beispiel 3: " + timeResult +
                " Mikrosekunden, Freier Speicher: " + Debug.GC(true) + "kb");
            avarageExample3 += timeResult;

            // Beispiel mit einer Devisions Rechnung
            // über eine Methode und den Integer referenzieren.
            Stopwatch.GetElapsedMillisecounds();

            result = 12345;
            GetDividedByRef(ref result);

            timeResult = Stopwatch.GetElapsedMicrosecounds();
            Debug.Print("Beispiel 4: " + timeResult +
                " Mikrosekunden, Freier Speicher: " + Debug.GC(true) + "kb");
            avarageExample4 += timeResult;

            // Beispiel mit einer Devisions Rechnung
            // über eine Methode aus einer anderen Klasse und den Integer kopieren.
            Stopwatch.GetElapsedMillisecounds();

            result = 12345;
            result = MethodeInOtherClass.GetDivided(result);

            timeResult = Stopwatch.GetElapsedMicrosecounds();
            Debug.Print("Beispiel 5: " + timeResult +
                " Mikrosekunden, Freier Speicher: " + Debug.GC(true) + "kb");
            avarageExample5 += timeResult;

            // Beispiel mit einer Devisions Rechnung
            // über eine Methode aus einer anderen Klasse und den Integer referenzieren.

            Stopwatch.GetElapsedMillisecounds();

            result = 12345;
            MethodeInOtherClass.GetDividedByRef(ref result);

            timeResult = Stopwatch.GetElapsedMicrosecounds();
            Debug.Print("Beispiel 6: " + timeResult +
                " Mikrosekunden, Freier Speicher: " + Debug.GC(true) + "kb");
            avarageExample6 += timeResult;

            if (countIteration > 100)
            {
                // Durchschnitts Ergebnisse
                Debug.Print("Beispiel 1, Durchscnittliche Zeit: " +
                    (avarageExample1 / countIteration) + " Millisekunde");
                Debug.Print("Beispiel 2, Durchscnittliche Zeit: " +
                    (avarageExample2 / countIteration) + " Mikrosekunden");
                Debug.Print("Beispiel 3, Durchscnittliche Zeit: " +
                    (avarageExample3 / countIteration) + " Mikrosekunden");
                Debug.Print("Beispiel 4, Durchscnittliche Zeit: " +
                    (avarageExample4 / countIteration) + " Mikrosekunden");
                Debug.Print("Beispiel 5, Durchscnittliche Zeit: " +
                    (avarageExample5 / countIteration) + " Mikrosekunden");
                Debug.Print("Beispiel 6, Durchscnittliche Zeit: " +
                    (avarageExample6 / countIteration) + " Mikrosekunden");
            }

            countIteration++;
        }
    }

   private static int GetDivided(int result)
    {
        return result / 67890;
    }
    private static void GetDividedByRef(ref int result)
    {
        result = result / 67890;
    }
}

public static class MethodeInOtherClass
{
    public static int GetDivided(int result)
    {
        return result / 67890;
    }
    public static void GetDividedByRef(ref int result)
    {
        result = result / 67890;
    }
}

image
Ermittelt eine Durchschnittszeit, da das Ergebnis schwankt.

Das erste Beispiel zeigt das Ergebnis in Millisekunden, jedoch reicht das für mein zeitkritisches Thema nicht aus. Daher zeigt das zweite Beispiel das Ergebnis in Mikrosekunden an. Sieht man sich die Durchschnittszeit an, dann passt diese Vorgangsrechnung praktisch 3 mal pro Millisekunde. Die Beispiele 3 bis 6 zeigen eher unwesentliche Unterschiede. Hier zeigt der Vorgang über eine Methode eine längere Zeitspanne von ca. 60 Mikrosekunden, egal wo sich die Methode befindet. Geht man die mehrmals errechneten Durchschnittszeiten durch, ist zu erkennen, dass die Ergebnisse schwanken. Vielleicht irre ich mich da, aber ich konnte erkennen, dass es nur beim Debuggen auftrat. Stabil blieben die Werte, nachdem ich die Ergebnisse immer über eine UART Verbindung an den PC gesendet habe und noch ein wenig besser wurden die Ergebnisse, wenn der Netduino über eine andere Spannungsversorgung von mindestens 7,5V angeschlossen wurde. Ich selbst verwendete ein 12V / 1A Schalt Netzteil oder einen 9V / mAh Block Akku für meine Anwendungen und Tests. Für die folgende Ausgabe habe ich noch 3 weitere Arten des Methodenaufrufes eingesetzt.

  • Beispiel 7 ist das Ergebnis über einen Methodenaufruf in einer anderen DLL.
  • Beispiel 8 ist das Ergebnis über einen Methodenaufruf mit zwei übergebenen Werten.
  • Beispiel 9 ist das Ergebnis über einen Methodenaufruf, wobei der referenzierte Integer vorher angelegt wird.
image
Ergebnis ohne Debug und Netduino weiterhin an USB angeschlossen.

Seltsamerweise war das Ergebnis unwesentlich besser.

image
Ergebnis mit einem 12V Schaltnetzteil, mit dem Akku sah die Ausgabe geradezu identisch aus.

Bei diesen Beispielen mit den verschiedenen Spannungsquellen, lohnt sich der Test allerdings erst, wenn der Netduino aufwendigere Aufgaben zu erledigen hat, auf dem weitere Module gesteckt sind und viele Algorithmen verwendet werden.

image
Akkus und Schaltnetzteile stellen zum Glück vernachlässigbare Unterschiede.

Zum Abschluss diesen Blog Eintrages bildet sich eines auf jeden Fall klar ab: Es kostet Zeit! Und je aufwendiger das Programm, um so exponentiell steigt die Möglichkeit der Optimierung und macht erst Sinn, wenn das Ziel diese Anforderung unausweichlich benötigt.

Beispiel Projekt zum Downloaden. (.NET Micro Framework 4.2)

Donnerstag, 30. Mai 2013

Beschleunigungswerte aus dem Sensor umrechnen

Quadrocopter_47
Die Sensoren MPU6050 und BMA020

Es gibt zwei Varianten, wie der Winkel errechnet werden kann. Als erstes Beispiel nehmen wir für einen Winkel von 90° einen imaginären Messbereich von 0 bis 100, der die Y Achse darstellt.

image_thumb[2]
Visuelle Darstellung der Achsenausrichtung.

Dann könnte die Formel “(MaxWert / 90°) x Messwert = Winkel” lauten. Die Umsetzung ist relativ naheliegend und kann schnell im Programmcode geschrieben werden.

// Variable mit dem Messwert
int reading = 0;
// Ergebnis = (Maximaler Wert / 90°) * Messwert
double result = (100 / 90) * reading;

Messbereich von 0 bis 100

Je nach Sensor ergeben sich andere maximale Wert. Beim MPU6050 geht der Messbereich von 0 bis 65535 und in Abhängigkeit, welche Einstellung gesetzt wurde, ergibt sich der “MaxWert”. Wenn z.B. der Sensor auf 8G initialisiert wurde, dann muss der Wert durch 8 geteilt werden und erhält somit den MaxWert = 8192. Dieser Wert wird erreicht, wenn die Achse 1G misst.

// Variable mit dem Messwert
int reading = 0;
// Ergebnis = (Maximaler Wert / 90°) * Messwert
double result = (8192 / 90) * reading;

Messbereich von 0 bis 8192

Hier bildet sich allerdings ein konstanter Nachteil. Der “MaxWert” muss immer für verschiedene Messbereiche neu gesetzt werden.

Eine geschicktere Lösung sind die Formellösungen für die Berechnung eines Dreiecks. Da jeder in der Schule im Matheunterricht Geometrie hatte, erspare ich die Erklärung, wie ich zu dieser Formelaufstellung gekommen bin.

image
Verwendete Formel

Das sieht etwas wild aus und benötigt auch im Programmcode einige Zeilen mehr. Im vorigen Beispiel habe ich die restlichen 270° ausgelassen, da eine weitere Erläuterung nicht weiter sinnvoll erschien.

private static int GetAngle(int a, int b)
{
    if (a != 0 & b != 0)
    {
       
// Angewendete Formel zur berechnung des Winkels
        // Winkel = ArkusSin((a² + c² - b² / 2ac) * (360 / Pi))

        double qA = System.Math.Pow(a, 2);
        double qB = System.Math.Pow(b, 2);
        double qC = qA + qB;

        // Das Ergebnis das unter dem Bruch entsteht, führte zu einem Fehler,
        // wenn ich dies alles in ein Zeile geschrieben habe.

       double divided = (2 * a * System.Math.Sqrt(qC));
       double result = (System.Math.Acos((qA + qC - qB) / divided) * (180.0 / System.Math.PI));

       if (a > 0 & b < 0)
        {
    
// Wenn Z nach unten ausgerichtet ist und Y(X) nach links(oder rechts) gekippt ist
            //   |
            // -- --
            // x |
            // -90 bis -180

            return (90 - ((int)result)) * -1;
        }
        else if (a > 0 & b > 0)
        {
    
// Wenn Z nach unten ausgerichtet ist und Y(X) nach rechts(oder links) gekippt ist
            //   |
            // -- --
            //   | x
            // 0 bis -90

            return ((int)result) - 90;
        }
        else if (a < 0 & b < 0)
        {
    
// Wenn Z nach oben ausgerichtet ist und Y(X) nach links(oder rechts) gekippt ist
            // x |
            // -- --
            //   |
            // 90 bis 180

            return ((int)result) - 180;
        }
   
// Wenn Z nach oben ausgerichtet ist und Y(X) nach rechts(oder links) gekippt ist
        //   | x
        // -- --
        //   |
        // 0 bis 90

       return 180 - (int)result;
    }
    else if (a == 0 & b != 0)
    {
        if (b < 0)
        {
            return -90;
        }
       else
        {
           return 90;
        }
    }
    else if (a != 0 & b == 0)
    {
       
// Wenn jedoch Y 0 ist und Z mehr als 1
       if (a > 0)
        {
            return -90;
        }
    }
    return 0;
}

Methode um aus Z und X (oder Y) den Winkel Y (oder X) zu berechnen.

Die Methode nimmt zwei Werte an. Für den Integer ‘a’ wird der Wert von der Beschleunigungssensorachse Z übergeben, der Integer für ‘b’ benötigt den Wert von der Achse X oder Y. Z und X ergeben den Winkel Y. Für Z und Y kommt der Winkel X heraus. Bevor jedoch die Messwerte durch die Methode geschickt werden, muss festgestellt werden, dass ein negativer Wert abgebildet wird. Daher ist es sinnvoll, den Sensor Wert mit der Hälfte des Maximalen Wertes zu verschieben.

private static int GetShiftValue(int value)
{
    if (value > 32768 && value < 65536)
    {
        value = 65536 - value;
    }
    else
    {
        value = 0 - value;
    }
    return value;
}

Mit der Methode wird aus 0 bis 65535, ein Bereich von –32767 bis +32767

Naja, hier muss dann doch am Ende der Maximale Wert gesetzt werden, dabei ist die Einstellung des Sensors wiederum egal. Bei dem Windows Phone 8 ist diese Methode nicht erforderlich.

Quadrocopter_46
Test auf dem Windows Phone 8 (Lumia 820)

Mit dieser Methode können die Daten von verschiedenen Sensoren genutzt werden, ohne dass jedes mal vorher ein Maximal Wert ermittelt und gesetzt werden muss.

Sonntag, 12. Mai 2013

DLPF einstellen für den MPU6050

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

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

image
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

Samstag, 11. Mai 2013

Abstandsmessungen mit dem Netduino

Quadrocopter_41

Links: HC-SR04 Ultraschall Sensor, Rechts: GP2-1080 Optischer Sensor

Für die Abstandsmessung kann wahlweise ein Ultraschall- oder ein optischer Sensor verwendet werden. Naja beides könnte man auch verwenden, es kommt aber auf die Anwendung an:

  • Welche Entfernungen muss ich messen können?
  • In welcher Umgebung wird agiert?
  • Wie sieht es mit Störfaktoren aus?
  • Wie viel darf der Sensor kosten?

Was nun von beidem besser ist, möchte ich hier nicht begründen, da beide Varianten eine Daseinsberechtigung haben. Für einen richtigen Vergleich hätte ich allerdings mehr einkaufen müssen, daher ist meine Vergleichstabelle nur als Ansatz gedacht.

  HC-SR04 GP2-1080
Betriebsspannung 5V 4,5V-5,5V
Stromaufnahme 15mA 33mA
Messung erfolgt Abstand von Trigger Puls bis Echo Puls Ergebnis wird als Analoges Spannungssignal ausgegeben
Distanzmessung 2cm - 400cm 10cm – 80cm
Nachteile Kann durch Geräusche gestört werden Funktioniert auf Reflektierenden Oberflächen nicht
Vorteile weite Distanz-Messung Kann leicht in Analoge Schaltungen verwendet werden.
Preis 1,86€ (Ebay, aus China Import), ab 4,99€ (Amazon)
7,05€ (Reichelt)

Im programmatischen Anteil sind beide recht unterschiedlich von der Handhabung. Wo es beim optischen Sensor geradezu ein Kinderspiel ist, zeigt sich der Ultraschallsensor wesentlich aufwendiger.

image
Schaltbild für den optischen Sensor

So einfach der Aufbau, so einfach ist auch der Programmcode.

public static void Main()
{
    AnalogInput ai = new AnalogInput(Cpu.AnalogChannel.ANALOG_0);

    while (true)
    {
        Debug.Print("ReadRaw:" + ai.ReadRaw().ToString());
        Thread.Sleep(10);
    }
}

Die Ausgabe zeigt einen Wert zwischen 0 bis 1000 an.

Ich glaube, dass ist bisher unter den Sensoren der übersichtlichste Programmcode, was ich bisher posten konnte. Im Grunde wird nur ein Pegel vom Infrarotsensor gemessen. Beim Ultraschallsensor ist nur die einfache Verkabelung, der Rest sieht daher leider anders aus.

image
Die Schaltung mit dem Ultraschall Sensor ist unwesentlich gegenüber dem optischen Sensor. (Leider gibt es in der Fritzing Software den Ultraschallsensor nicht.)

Für den Ultraschallsensor habe ich versucht, so weit wie möglich zu kürzen. Naja, das ist dabei herausgekommen:

private static long _End;

public static void Main()
{
    OutputPort transmitter = new OutputPort(Pins.GPIO_PIN_D12, false);
    InterruptPort receiver = new InterruptPort(Pins.GPIO_PIN_D11, false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeLow);

    receiver.OnInterrupt += new NativeEventHandler(receiver_OnInterrupt);

    long begin;
    long result = 0;

   while (true)
    {
        // Sende Ping
        transmitter.Write(true);
        Thread.Sleep(1);

       // Werte auf Startpunkt festlegen
        _End = 0;
        begin = DateTime.Now.Ticks;
        transmitter.Write(false);

       // Bis in 50 Millisekunden sollte die Messung erfolgt sein.
        Thread.Sleep(50);

        if (_End > 0)
        {
           // Differenz von Start und Stopzeit berechnen
            result = _End - begin;
            if (result < 0)
            {
                result = 0;
            }
        }

        Debug.Print("Result: " + result.ToString());
        Thread.Sleep(100);
    }
}
// Wird ausgeführt, wenn der Ping Empfangen wurde.
private static void receiver_OnInterrupt(uint data1, uint data2, DateTime time)
{
    _End = time.Ticks;
}

Deutlich mehr als das, was man vermutet.

Wenn man sich unbelastet und die Sache als Unerfahrener von außen betrachtet, würde man davon ausgehen, dass die vielen Chips auf dem HC-SR04 bereits diese Arbeit abnimmt. Wer nicht viel drum herum programmieren möchte, der sollte zumindest eine fertige Klasse schreiben. Wer Threads verwendet, der sollte berücksichtigen, dass es zu Messfehlern kommen kann.

Fazit: Wer einen großen Messbereich für wenig Geld benötigt, für den ist der Ultraschallsensor das Richtige. Für einfache und präzise Anwendungen dürfte der optische Sensor die ideale Lösung sein.

Mittwoch, 1. Mai 2013

PWM Signal Einlesen mit dem Netduino

PwmSignal_Netduino2
Der Empfänger am Netduino

Es gibt zwei Lösungen, wie man ein PWM Signal einlesen kann. Über eine RC Schaltung mit der “AnalogInput” Klasse oder vollständig programmatisch. Wenn keine Bauteile wie Widerstand und Kondensator vorhanden ist, dann erübrigt sich das letztere. Beim Arduino kennt man bereits aus der Library die Methode ‘pulseIn()’. Leider wurde das für den Netduino noch nicht umgesetzt. Alternativ bleibt die Möglichkeit, die sich ändernden Pulszustände zeitlich zu messen und das geht hervorragend mit der “InterruptPort” Klasse.

/// <summary>
/// Dieses Klasse ermöglicht das genaue Einlesen des eingehenden Pulssignals
/// </summary>
private static InterruptPort _IntPort = new InterruptPort(
    Pins.GPIO_PIN_D0,
    true,
    Port.ResistorMode.Disabled,
    Port.InterruptMode.InterruptEdgeBoth);

/// <summary>
/// Wird verwendet um den Startzeit des Pulssignals zu merken
/// </summary>
private static long _HighTicks = 0;
/// <summary>
/// Kann verwendet werden um einen Servo anzusteuern
/// </summary>
private static PWM _PwmOutput = new PWM(Cpu.PWMChannel.PWM_0, 20000, 1500, PWM.ScaleFactor.Microseconds, false);

public static void Main()
{
    // PWM Ausgang starten
    _PwmOutput.Start();
    // Event zuweisen
    _IntPort.OnInterrupt += new NativeEventHandler(_IntPort_OnInterrupt);

    while (true)
    {
        Thread.Sleep(1000);
    }
}
/// <summary>
/// Wird ausgeführt, wenn sich der Zustand am Eingang geändert hat.
/// </summary>
private static void _IntPort_OnInterrupt(uint data1, uint data2, DateTime time)
{
   if (data2 == 1)
    {
        // Startzeit des Pulssignals merken
        _HighTicks = time.Ticks;
    }
    else
    {
        // Mit Ende des High Zustands wird die aktuelle
        // Zeit mit der begin Zeit abgerechnet und erhält hier
        // die Pulslänge des High Signals

        double result = time.Ticks - _HighTicks;

        // Werte werden zwischen 10.000 bis 20.000 ausgegeben
        Debug.Print(result.ToString());

       if (result < 20000)
        {
            // Das Pulssignal darf nur zwischen 1000 bis 2000 sein
            _PwmOutput.Duration = (uint)(result / 10);
        }
    }
}

Zum einlesen eines PWM Signals

Über die PWM Klasse gebe ich das eingelesene Signal wieder aus, um z.B. einen Servo oder Regler anzusteuern. Sinn ergibt dies jedoch, wenn der Wert noch verarbeitet wird, bevor es weiter gegeben wird.

Solution zum Downloaden: ExampleReadPwm

Ameisen Simulation und andere Dinge

Wer lange sich schon mit C# beschäftig, hat sicherlich schon mal was von AntMe gelesen oder gehört. Diese Idee hatte ich aufgegriffen und mi...