261 lines
14 KiB
Markdown
261 lines
14 KiB
Markdown
# Aufgabe 4 - IMU
|
||
|
||
Nachdem in der vorherigen Aufgabe das Thema JTAG bereits angerissen wurde, soll es
|
||
auch in Aufgabe 4 behandelt werden. Es wird eine weitere Komponente des Anwendungsfalls debuggt. Zusätzlich zum JTAG-Debugging wollen wir Ihnen einige Funktionen des
|
||
Lauterbach-Debuggers näher bringen.
|
||
Damit der Roboter jederzeit seinen Winkel zur Horizontalen kennt, benötigt er eine
|
||
echtzeitfähige Lagemessung. Der Treiber für die Ansteuerung der inertialen Messeinheit
|
||
über I2 C wird mit Hilfe des JTAG-Debuggings und der Nutzung des Lauterbach Logik
|
||
Analysators in einen fehlerfreien Zustand gebracht. Die Werte des Sensors werden den
|
||
anderen Programmmodulen über globale Variablen zur Verfügung gestellt.
|
||
Die Aufgabe ist es, ein Programm zu schreiben, dass die aktuellen Werte des Gyroskops
|
||
und des Beschleunigungssensors ausliest und in zwei globalen Variablen speichert. Maßgeblich für die Funktion des Moduls ist die korrekte Initialisierung des I2 C Busses, die
|
||
richtige Konfiguration der inertialen Messeinheit und das korrekte Umwandeln der Daten. Bei dieser Aufgabe wird neben dem JTAG-Debugging unterstützend ein Logik Analysator genutzt. Die Studierenden sollen mit Hilfe des Lauterbach Logic Analysers die
|
||
übertragenen Daten auf dem I2 C Bus einsehen und die gesendeten Informationen extrahieren. Zur Kontrolle kann die von Lauterbach zur Verfügung gestellte automatische
|
||
Protokollanalyse des Logik Analysators genutzt werden.
|
||
|
||
## 8.1. Wissen
|
||
### 8.1.1. Inertiale Messeinheit und Sensorfusion
|
||
|
||
Die MPU9250 von InvenSense bietet eine 9-Achsen Messeinheit mit Accelerometer, Gyroskop und Magnetometer. Die MPU9250 wird auch als Bewegungs- und Lagesensor in
|
||
Smartphones genutzt. Die aus der Sensoreinheit ausgelesenen Werte müssen für die weitere Verwendung bearbeitet werden. Dabei ist es sinnvoll sich auf die Kippachse nach
|
||
vorne und hinten zu konzentrieren. Um die Lage richtig einschätzen zu können, benötigt
|
||
man zwei Messwerte von dem Sensor. Die Beschleunigung sowie die Winkelgeschwindigkeit. Um einen möglichst fehlerfreien Wert zu bekommen, müssen die Messwerte gefiltert
|
||
werden. Die Probleme, die sich dabei auftun, sind folgende: Der Beschleunigungssensor
|
||
ist sehr anfällig für Rauschen, also für kurzfristige Fehler. Dafür kann er die Winkelmessung nicht relativ, sondern absolut ausführen. Die Winkelgeschwindigkeit wird sehr
|
||
genau gemessen und kaum von äußeren Einflüssen gestört. Der aus dem Gyroskop resultierende Winkel unterliegt durch die Integration der Messwerte einem gewissen Drift. Es
|
||
ist nötig diese Sensordaten zu filtern und zu kombinieren, um die Schwächen der beiden
|
||
einzelnen Methoden damit auszugleichen.
|
||
|
||
Neben der in Aufgabe 3 erlernten Arbeitsweise mit JTAG-Debugging, ist Grundlagen-
|
||
wissen über die Funktionsweise von I2 C nötig. Dazu können Sie sich [[i2c](references.md)] ansehen. Es
|
||
sollte bekannt sein, wie ein I2 C Gerät angesprochen wird, welche Pins dafür nötig sind
|
||
und wie Register im Zielgerät gelesen oder geschrieben werden. Für die Analyse mit dem
|
||
Lauterbach Logic Analyser finden sich in [[Lau14](references.md), S. 48-49] Informationen zur automatischen Protokollanalyse von I2 C. Weitere Informationen sind im Datenblatt [[Inva](references.md)] und der
|
||
Register Map [[Invb](references.md)] der inertialen Messeinheit zu finden. Sie sollten über Wissen zu inertialen Messeinheiten verfügen und sich darüber im Klaren sein, warum eine Sensorfusion nötig ist und wie man sie realisiert. Informationen dazu können Sie aus der folgenden
|
||
Quelle beziehen [[HiB](references.md)].
|
||
|
||
### 8.1.2. Visualisierung der Daten
|
||
|
||
Der Lauterbach-Debugger und die Trace32-Software bietet eine Vielzahl verschiedener
|
||
Funktionen. Beim Arbeiten mit Sensoren, die mit hoher Frequenz mehrere Messwerte
|
||
produzieren, kann es sehr hilfreich sein die Daten zu visualisieren.
|
||
Eine IMU ist ein solcher Sensor. Bei mehreren Achsen und wenigen zehn Hertz Messfrequenz ist die Ausgabe mit printf nahezu unbrauchbar, da der Entwickler kaum mit dem
|
||
Lesen der Werte hinterher kommt. Ganz abgesehen von dem großen Einfluss von printf
|
||
auf das Zeitverhalten.
|
||
|
||
Es bietet sich daher an die Daten graphisch in einem Koordinatensystem darzustellen.
|
||
Der Lauterbach ermöglicht es auf Variablen zuzugreifen und diese zu visualisieren. Dazu
|
||
speichert man in die Messwerte in seinem Programm über eine gewisse Zeit in einem
|
||
Array ab. Anschließend hält man sein Programm durch den Debugger an.
|
||
|
||
Zum einen kann man nach dem Array in dem “Symbol.browse”-Fenster suchen, die
|
||
Variable zur Watch-List hinzufügen und sich die Werte textlich anzeigen lassen.
|
||
|
||
Zum anderen kann man das Array plotten, indem man in die Lauterbach-Befehlszeile
|
||
folgendes eingibt:
|
||
|
||
```
|
||
var.DRAW <NAME_OF_ARRAY>
|
||
```
|
||
|
||
Der Befehl ermöglicht es auch mehrere Arrays in ein Fenster zu plotten, um Zusammenhänge zwischen Daten besser zu verstehen:
|
||
|
||
```
|
||
var.DRAW <NAME_OF_ARRAY_1> <NAME_OF_ARRAY_2>
|
||
```
|
||
|
||
## 8.2. Pre-Kolloquium
|
||
|
||
- Wie liest man aus einem I2 C Gerät?
|
||
- Welche Register aus der IMU sind interessant für uns? Schauen Sie sich das Registerdatenblatt der IMU an und suchen Sie Register, die für unsere Anwendung
|
||
interessant sind
|
||
- Wie werden rauschende Signale geglättet?
|
||
- Was wollen wir aus der IMU lesen?
|
||
- Müssen wir die IMU erst aufwecken?
|
||
- Wie konfiguriert man Gyroskop und Accelerometer so, dass beim Gyroskop Dps =
|
||
500 ist, ACC Skala = 4g ?
|
||
- Mit welcher Formel berechnet man den Winkel aus ACC und Gyroskop-Daten? Wo
|
||
ist der Unterschied zwischen beiden? Wo spielt die Sampling-Zeit mit hinein?
|
||
- Werten Sie den gegebenen I2 C Datenstrom (siehe 8.1) aus und geben Sie Adresse und
|
||
Inhalt der Nachricht wieder, handelt es sich um Lesen oder um Schreiben? (Tipp:
|
||
Abgebildet ist nur der Datenstrom ausgehend vom Master)
|
||
- Warum benötigt man zur sicheren Winkelberechnung eine Sensorfusion aus den
|
||
Daten von ACC und Gyroskop? Warum reicht ein Sensor hier nicht aus? (Machen
|
||
Sie sich mit dem Komplementärfilter vertraut)
|
||
|
||
 <br> Abbildung 8.1. I2 C Datenstrom zum Auswerten
|
||
|
||
## 8.3. Aufgabe
|
||
|
||
In dieser Aufgabe sollen Sie folgenden Ablauf schrittweise implementieren:
|
||
|
||
- I2 C initialisieren
|
||
- IMU initialisieren
|
||
- IMU-Task starten
|
||
- Periodisch IMU-Daten abfragen
|
||
- Winkel aus Accelerometer- und Gyro-Daten berechnen
|
||
- Berechneten Winkel an PID Filter propagieren
|
||
|
||
### IMU-Task erstellen
|
||
|
||
Die imu.c soll zwei Methoden nach außen bereitstellten. Diese lauten:
|
||
|
||
```
|
||
int mpu9250_Imu_Init(void ∗pdata); // Initialize
|
||
int mpu9250_CalculateAngle(void ∗pdata); // Task function
|
||
```
|
||
|
||
Erstellen Sie in der main.c einen neuen 5-Millisekunden-Task und starten Sie ihn so,
|
||
wie in Aufgabe 2 gelernt. Dieser soll das IMU Modul zunächst initialisieren und dann
|
||
in der while(1) Schleife die Berechnung des Winkels periodisch aufrufen. Füllen Sie in
|
||
der weiteren Aufgabe diese beiden Methoden mit allem was zur Initialisierung und zur
|
||
Berechnung benötigt wird aus.
|
||
|
||
### I2 C initialisieren
|
||
|
||
Erstellen Sie in der imu.c eine Methode zur Initialisierung der I2 C Instanz:
|
||
|
||
```
|
||
static uint8_t mpu9250_Iic_Init();
|
||
```
|
||
|
||
- Zunächst die globale Variable für die I2 C Instanz erstellen
|
||
|
||
```
|
||
static XIicPs Iic ;
|
||
```
|
||
|
||
- Zur I2 C Initialisierung: Config Struct erstellen
|
||
|
||
```
|
||
XIicPs_Config ∗Config;
|
||
```
|
||
|
||
- Config-Struct mit XIicPs Config füllen, (Tipp: Informationen zu Übergabeparametern finden sich in der imu.h)
|
||
|
||
```
|
||
Config = XIicPs_LookupConfig(...) ;
|
||
```
|
||
|
||
- I2 C initialisieren und Status abfragen, übergeben Sie der Methode die benötigten
|
||
Parameter (Tipp: Basisaddresse des I2 C findet sich auch in dem Config-Struct)
|
||
|
||
```
|
||
XIicPs_CfgInitialize (...) ;
|
||
```
|
||
|
||
- Zur Sicherheit einen Self-Test machen und das Ergebnis abfragen (in die Methode
|
||
gucken, um den Rückgabewert interpretieren zu können)
|
||
|
||
```
|
||
XIicPs_SelfTest (...) ;
|
||
```
|
||
|
||
- I2 C Clockrate setzen, suchen Sie in der imu.h nach Übergabeparametern. Achtung:
|
||
Die Clockrate soll 100kHz betragen.
|
||
|
||
```
|
||
XIicPs_SetSClk (...) ;
|
||
```
|
||
|
||
Das Nutzen der I2 C Schnittstelle erfolgt über zwei Methoden zum Senden und Empfangen von Daten aus den Registern des MPU9250 Sensors. Erstellen und implementieren
|
||
Sie die beiden benötigten Methoden in der Datei imu.c.
|
||
|
||
```
|
||
static uint8_t mpu9250_Write_Reg(uint8_t iic_address, uint8_t data)
|
||
static int8_t mpu9250_Read_Data(uint8_t iic_address, uint8_t length, u8 RecvBuffer [])
|
||
```
|
||
|
||
Die benötigte Schnittstelle des I2 C Treibers von Xilinx sind die Methoden
|
||
|
||
```
|
||
XIicPs_MasterSendPolled()
|
||
```
|
||
|
||
und
|
||
|
||
```
|
||
XIicPs_MasterRecvPolled()
|
||
```
|
||
|
||
Schauen Sie sich die Methoden an und identifizieren Sie die benötigten Übergabepara-
|
||
meter. Implementieren Sie auch hier eine Fehlerüberprüfung.
|
||
|
||
### IMU initialisieren
|
||
|
||
Nach der erfolgreichen Initialisierung der I2 C Schnittstelle ist es notwendig die inertiale
|
||
Messeinheit zu initialisieren. Nutzen Sie dafür die zuvor implementierten Send- und Recv
|
||
Methoden, um die Register der IMU wie gewünscht zu konfigurieren. Die benötigten
|
||
Register haben Sie im Präkolloquium ausgearbeitet. Zu überprüfen, ob die Initialisierung
|
||
erfolgreich war, ist nicht zwingend notwendig, hilft im Zweifel aber Fehler zu finden. Hier
|
||
kann der Status der IMU einmal abgefragt werden.
|
||
|
||
### IMU-Daten abfragen
|
||
|
||
Nach dem Initialisieren der IMU sollen Sie nun eine Methode zum Auslesen der Daten
|
||
von Accelerometer und Gyroskop erstellen. Lesen Sie die Daten der IMU an einem Stück
|
||
aus und vermeiden Sie schnell aufeinander folgende Lesevorgänge. Es lassen sich alle
|
||
Register mit den benötigten Rohwerten mit einem Lesevorgang auslesen.
|
||
|
||
Falls beim Auslesen Fehler auftreten, ist es sinnvoll den Status des Sensors auszulesen.
|
||
Vergleichen Sie dazu den Inhalt des Statusregisters mit dem angegebenen Sollwert. Gehen Sie systematisch alle Fehlerquellen durch, die die Kommunikation mit dem Sensor
|
||
unterbinden könnten. Einen Fehler in der Hardware können Sie ausschließen.<br>
|
||
Wenden Sie das Wissen aus 8.1.2 an, um die Daten von Accelerometer und Gyroskop mit
|
||
der Trace32-Software zu plotten. Schalten Sie den Lowpass Filter vom Accelerometer an
|
||
und aus und vergleichen Sie die Qualität der Messwerte.
|
||
Nutzen Sie den Lauterbach Logik Analysator (Untermenü ‘Probe‘->‘Timing‘) um die I2 C
|
||
Übertragung zu inspizieren. Verifizieren Sie die Rohwerte durch den Übungsleiter.
|
||
|
||
***Tipp: Überlegen Sie genau welche Sensorachsen Sie für die Winkelberechnung im weiteren Verlauf der Aufgabe benötigen und welche nicht. Vergleichen Sie hierzu die Grafik
|
||
zur Ausrichtung der Achsen im Datenblatt der IMU mit der Lage auf dem Roboter im
|
||
Versuchsaufbau.***
|
||
|
||
### Winkel berechnen
|
||
|
||
Außerdem müssen die Accelerometerdaten skaliert werden. Für die Winkelberechnung
|
||
ist es nötig, dass das Accelerometer die Erdbeschleunigung auf jeder der drei Achsen
|
||
gleich misst (von Werk aus nicht der Fall). Finden Sie zunächst die Maximal- und Minimalwerte für jede Achse bezüglich der Erdbeschleunigung heraus. Dies können Sie
|
||
tun, indem Sie den Roboter langsam um jede Achse drehen dabei jede Messung mit den
|
||
_imu_accMaxData und _imu_accMinData Arrays vergleichen und diese ggf. aktualisieren.
|
||
Notieren Sie sich diese Messwerte und nutzen Sie sie im Folgenden als Konstanten für die
|
||
Grenzen des ACC Wertebereiches. Mappen Sie anschließend die Daten der einzelnen Achsen des Accelerometers auf einen konstanten Wertebereich, anhand dieser Messdaten. (z.B. InMin = INT16_MIN ; inMax = INT16_MAX)<br>
|
||
***Tipp: Schauen Sie sich hierfür typische Map-Funktionen an und mappen Sie linear!***
|
||
|
||
Die Gyroskopdaten müssen mit einem
|
||
passenden Offset kalibiert werden. Beachten Sie außerdem den entsprechenden sensitivity scale factor (Stichwort: GYRO_FS_SEL) beim Ermitteln der Daten des Gyroskop.<br>
|
||
***Tipp: Schauen Sie hierfür nicht nur in die Register Map, sondern auch in das entsprechende Datenblatt.***
|
||
|
||
Im Folgenden sollen die gemessenen Werte in eine Winkelangabe transformiert werden. Berechnen Sie zwei Winkel: Der erste soll auf Basis der Accelerometerdaten (Stichwort: atan2()) berechnet werden, der zweite auf Basis der Gyrodaten (Stichwort: Winkelgeschwindigkeit -> Winkel). Überprüfen Sie die beiden Winkel auf ihre Plausibilität. Zusätzlich benötigen Sie noch die Winkeländerung pro Zeitschritt.<br>
|
||
***Tipp: In unserem Aufbau zeigen die Winkel des Accelometers und des Gyroskops in die jeweils gegenüberliegende Richtung!***
|
||
|
||
Benutzen Sie die oben genannten Winkel und implementieren Sie einen Komplementärfilter, der die beiden Winkel zu einem fusioniert. Dieser sollte wie in etwa so aussehen:
|
||
|
||
```
|
||
currentAngle = 0.98∗(previousAngle + gyroAnglePerTimeStep) + 0.02∗accAngle;
|
||
```
|
||
|
||
In Aufgabe 7 wird Ihnen ein PID-Regler vorgegeben werden, damit die Motoren des
|
||
Roboters adäquat auf den aktuellen Winkel reagieren. Die Kommunikation mit dem PID-
|
||
Regler-Task läuft über eine globale Variable.
|
||
|
||
Tipps:
|
||
|
||
- Achten Sie auf das Einfügen der Datei in dem richtigen Ordner.
|
||
- Schauen Sie sich die IMU an und interpretieren Sie die Achsausrichtung, um herauszufinden welche Achsen für die Berechnung der Winkel aus Accelerometer- und
|
||
Gyroskopdaten nötig sind.
|
||
- Was macht die Funktion atan2()? Zu welcher Library gehört sie? Es gibt eine Besonderheit die zur Verwendung der Library erfüllt sein muss, welche ist es?
|
||
- Wie erstellt man aus dem Gyroskopwert dem aktuellen Winkel? Führen Sie für die
|
||
Sample Time eine Konstante ein, die später an die echte Ausführungsperiode an-
|
||
gepasst werden kann
|
||
|
||
## 8.4. Post-Kolloquium
|
||
Bereiten Sie sich auf das Kolloquium vor, indem Sie sich die Arbeitsergebnisse und gefundenen Fehler gut dokumentieren. Verstehen Sie, was Sie programmiert haben.
|
||
|
||
- Sie haben für die I2 C Kommunikation die Funktion XIicPs_MasterSendPolled() und XIicPs_MasterRecvPolled() verwendet. Wo ist der Unterschied zu den Funktionen XIicPs_MasterSend() und XIicPs_MasterRecv()?
|
||
- Nennen Sie neben I2 C noch weitere simple Busprotokolle, welche in Eingebetteten-Systemen verbreitet sind. Wie unterscheiden diese sich?
|
||
- Mit dieser Aufgabe haben Sie erstmals ein Echtzeitverhalten implementiert. Wo liegt dies, welche Zeitschranken müssen eingehalten werden und was könnte passieren wenn das diese nicht eingehalten werden sollten?
|
||
- Wodurch könnte dieses Zeitverhalten verletzt werden?
|
||
|
||
|