Da die Software auf die serielle Schnittstelle, an welcher der Kompaß angeschlossen ist zugreifen muß, ist natürlich sicherzustellen, daß Lese- und Schreibzugriff auf die entsprechende Gerätedatei gewährt wird.
Die Variable marciefdflag, die standartmäßig auf -1 gesetzt ist wird auf 0 gesetzt, um anzuzeigen, daß die Schnittstelle geöffnet ist.
Zuletzt wird noch die Funktion marcie_closeserial() als exit-Funktion eingetragen, d.h. Diese Funktion wird beim Beenden des Programms ausgeführt.
marcie_closeserial() schaltet beim Kompaß die kontinuierliche Messung aus. Das verhindert das Auftreten eines falschen Meßwertes nach folgendem Szenario:
Nehmen wir an, der Kompaß wäre noch aktiv und würde Daten liefern während die Schnittstelle gerade geöffnet wird. Alle noch im Schnittstellenpuffer befindlichen Daten werden dabei automatisch gelöscht. Danach wird der Kompaß auf Messung auf Anfrage geschaltet. Die laufende Messung wird aber noch durchgeführt, so daß nach deren Ende ein Meßwert geschickt wird, der natürlich nicht erwartet wird und beim weiteren Polling die Werte um einen Satz verschiebt.
Nach dem Abschalten des Kompasses setzt die Funktion die Schnittstelle auf die alten Einstellungen zurück und schließt die Gerätedatei.
marcie_sendcommand() schickt ein Kommando bestehend aus einem Befehl (A..F) und einem Argument (0..255) an den Kompaß. Diese Funktion wird von den folgenden Funktionen benutzt.
marcie_setmeascount() stellt die Anzahl der Messungen zwischen jeweils zwei Flip-Pulsen ein. Diese Anzahl wird auch in marciemeascount gespeichert, um bei der Berechnung der Gültigkeit eine Referenzamplitude zu haben, da die gemessene Amplitude natürlich mit zunehmender Anzahl sich addierender Messungen größer wird.
marcie_setmeasspeed() setzt die Meßgeschwindigkeit in 0.56ms-Schritten. Dabei ist die eingestellte Zeitspanne jeweils das Intervall zwischen dem Flip-Puls und der ersten Messung, zwischen den einzelnen Messungen, sowie zwischen der letzten Messung und dem nächsten Flip-Puls.
marcie_requestmeas() fordert eine Messung an. Bei eingeschalteter kontinuierlicher Messung hat diese Funktion keine Wirkung.
marcie_setcontmeas() schaltet zwischen kontinuierlicher Messung und Messung auf Anfrage um.
marcie_setclearclock() setzt die Kompaßuhr auf null.
marcie_ledonoff() schaltet die LED im Kompaß ein bzw. aus.
marcie_installhandler() richtet einen Signal-Handler ein, der jedes Mal, wenn ein Meßwert vom Kompaß vorliegt aufgerufen wird. Dazu wird zunächst überprüft, ob die Gerätedatei bereits geöffnet ist und noch kein Handler installiert ist. Das ist genau dann der Fall, wenn marciefdflag null ist.
Danach wird der Handler eingerichtet und der Dateizugriff auf asynchron geschaltet. marciefdflag wird dann mit FASYNC belegt um anzuzeigen, daß bereits ein Handler installiert ist und um bei späterem Umkonfigurieren der Schnitstelle den asychronen Modus beizubehalten.
marcie_getrawvalue() liefert die Meßwerte, bestehend aus Status, Nord-Süd-Wert, Ost-West-Wert und Uhr, wie sie vom Kompaß kommen. Wenn Blocking eingeschaltet ist, also die Funktion erst zurückkehren soll, wenn ein Meßwert empfangen wurde, dann wird der Dateizugriff entsprechend konfiguriert. Nun wird versucht, ein Zeichen zu lesen.
Liegt kein Zeichen an, und ist Nonblocking-Mode eingeschaltet, so kehrt die Funktion zurück, ansonsten wartet sie, bis ein Zeichen anliegt. Ist das gelesene Zeichen etwas anderes als der Header der Meßdaten vom Kompaß, so wird das Zeichen ignoriert und auf das nächste gewartet. Das stellt einen Schutz gegen falsche Synchronisation mit den Kompaßdaten dar, denn falls ein Byte verloren geht, würden die Meßdaten verschoben ankommen und daher falsch interpretiert. Wenn aber nun in jedem Fall auf den Datenheader gewartet wird, so kann ein auftretender Synchronisationsfehler wieder beseitigt werden, was anderweitig nicht möglich wäre. Natürlich können die Meßdaten zufälligerweise auch ein »A« enthalten, auf das nach einem Synchronisationsverlust dann falsch synchronisiert würde, aber durch die sich ändernden Daten ist es unwahrscheinlich, daß das über mehrere Meßwerte hin passiert.
Sobald also der korrekte Header empfangen wurde, wird der Dateizugriff in jedem Fall auf Blocking geschaltet, da die restlichen Bytes in jedem Fall gelesen werden sollen.
Diese bestehen aus einem Status-Byte, jeweils zwei Byte für NS- und OW-Wert, die danach in einen signed short int umgewandelt werden und vier Byte für die Kompaßuhr, die zu einem unsigned long int zusammengefügt werden.
marcie_loadcaltab() lädt die Kalibriertabelle in den Speicher. Für den Fall, daß kein Dateiname angegeben ist, wird er auf einen Standard-Wert gesetzt, anschließend wird die Datei geöffnet.
Läßt sich die Datei normal öffnen, so wird sie eingelesen. Dabei wird zunächst der Datei-Header übersprungen und dann der Nord-Süd- und der Ost-West-Gleichanteil gelesen. Anschließend folgen zeilenweise 361 Werte bestehend aus dem realen Winkel und einem Amplitudenwert für diesen Winkel. Zuletzt wird die Datei wieder geschlossen.
marcie_getangle() liefert den unkalibrierten Winkel, sowie Status, Gültigkeit und Uhr. Diese Funktion benutzt marcie_getrawvalue() um die Meßwerte zu erhalten und rechnet die Nord-Süd- und die Ost-West-Werte in Winkel und Gültigkeit um. Die Gültigkeit ist 1 für den Fall, daß die Magnetfeldamplitude genau einem Wert von 500 mal der Anzahl der durchgeführten Messungen pro Flip-Puls entspricht und wird kleiner, je weiter die Amplitude von diesem Wert entfernt ist.
marcie_getcalangle() gibt den kalibrierten Winkel, zusammen mit dem Status, der Gültigkeit und der Uhr zurück. Dazu wird auch hier mit marcie_getrawvalue() der Meßwert vom Kompaß gelesen und daraus ein Winkel und eine Amplitude berechnet. Dieser Winkel wird nun mit Hilfe der Kalibriertabelle in den realen Winkel umgerechnet. Liegt der gemessene Winkel zwischen zwei Tabelleneinträgen, so wird zwischen diesen linear interpoliert. Die Gültigkeit ist 1, falls der gemessene Wert der Amplitude aus der Kalibiertabelle bei diesem Winkel entspricht und wird kleiner, je weiter die beiden Werte auseinanderliegen.
Nun wird der Kompaß initialisiert. Dabei wird eine hohe Meßgeschwindigkeit und volle Genauigkeit eingestellt. Der Benutzer versetzt nun den Roboter in Rotation und startet durch einen Tastendruck die Messung.
Während der folgenden etwa 2 ½ Umdrehungen des Roboters werden in konstanten Zeitintervallen die Meßwerte aufgezeichnet. Ist die Rotation beendet, so wird überprüft, ob eine gewisse Mindestanzahl an Meßwerten erhalten wurde. Wenn dies sichergestellt ist, so wird eine Autokorrelation der NS- und der EW-Werte durchgeführt. Das Mimimum des Autokorrelationswertes wird bestimmt, um die Periode des Signals zu ermitteln. Durch einen Offset bei der Autokorrelation wird garantiert, daß nicht das erste, sondern das zweite Minimum gefunden wird.
Nun wird auch klar, warum mindestens zwei, jedoch weniger als drei Umdrehungen lang gemessen werden muß. Bei weniger als zwei Umdrehungen kann die Periode nicht bestimmt werden und bei mehr als drei Umdrehungen kann es passieren, daß die erste mit der dritten Umdrehung besser korreliert, als mit der zweiten und daher das Programm die doppelte Periode annimmt.
Ist die Periode bestimmt, so wird innerhalb dieser Periode der Gleichanteil berechnet, und anschließend vom gespeicherten Signal abgezogen. Dabei wird auch der resultierende Winkel berechnet.
Das gespeicherte Signal wird nun nach dem Nulldurchgang des Ost-West-Wertes bei negativen Nord-Süd-Wert durchsucht. Dieser Punkt wird im folgenden als Norden angenommen.
Angefangen mit diesem Punkt wird nun eine Kalibriertabelle angelegt, die jedem gemessenen Winkel seinen realen Winkel zuweist. Da die Rotation des Roboters als konstant angenommen wird, ist die Position des Winkels in der Liste der aufgenommenen Meßwerte proportional zum Winkel. Nachdem auch die Anzahl der Meßwerte pro Umdrehung und der Nullpunkt, sprich der Norden-Eintrag bekannt ist, kann aus der Position des Meßwerts exakt der echte Winkel berechnet werden.
Aufgrund der Tatsache, daß die Kalibriertabelle natürlich nur endlich viele Einträge hat, müssen die gemessenen Winkel auf ganze Zahlen gerundet werden, um die Zeile für einen Eintrag zu liefern. Um dem so entstehenden Fehler Rechnung zu tragen, wird der an dieser Stelle eingetragene reale Winkel mit einem Gewichtsfaktor versehen, der von eins ab kleiner wird, je weiter der gemessene Winkel von dem gerundeten Wert entfernt ist. Nach der Gewichtung wird dieser reale Winkel in der Zeile der Tabelle, die sich aus dem gemessenen Winkel ergibt, ebenso wie sein Gewichtungsfaktor in je einer Spalte aufaddiert. Das Speichern des Gewichtungsfaktors ist nötig um den Winkel später wieder von seiner Gewichtung befreien zu können.
Außerdem ist auf die Unstetigkeitsstelle in der Tabelle zwischen 360 und 0 Grad zu achten. Es kann bei der Messung durch geringe Meßfehler durchaus passieren, daß ein gemessener Winkel von knapp über 0° einem realen von knapp unter 360° entspricht oder umgekehrt. Würden nun in einer Zeile der Kalibriertabelle einige Winkel von etwa 360° mit einigen von 0° verrechnet, so ergäben sich völlig unsinnige Einträge. Um dies zu verhindern, erfolgt eine Plausibilitätsprüfung der Werte, bevor sie eingetragen werden.
Die so gewonnene Kalibiertabelle wird anschließend nach fehlenden oder zu ungenauen (kleiner Gewichtsfaktor) Einträgen durchsucht. Wird ein solches Loch gefunden, so versucht das Programm, dieses durch lineare Interpolation der benachbarten Werte zu gewinnen, falls diese stark genug sind. Ist auch einer der beiden Nachbarwerte zu ungenau, so gibt das Programm eine Meldung aus und die Messung muß mit geringerer Rotationsgeschwindigkeit wiederholt werden.
Zum Schluß werden die Einträge wieder von ihrer Gewichtung befreit und zusammen mit den Gleichanteilen für Nord-Süd und Ost-West in eine Datei geschrieben.
testraw() testet das direkte Lesen vom Kompaß. Diese Funktion wird auch für den Abgleich auf gleiche Sensoramplituden (siehe Anhang C.1) benötigt.
testnormal() liest die berechneten Winkel und Gültigkeiten aus und zeigt die an.
testcalib() stellt die realen Winkel dar und testsignal() ist ein Test für die asynchrone Bearbeitung der Daten.
Das Programm läuft, bis es durch einen
Tastendruck beendet wird.