sitemap link mailform link home

Kompass-Programmierung

Hier einige Erläuterungen zur Ermittlung der Himmelsrichtungen

Die Messwerte X_VALUE un Y_VALUE müssen nun noch in Himmelsrichtungen bzw. Gradzahlen umgewandelt werden.

Kompassrose

Die Messwerte meines HDMM01-Moduls liegen etwa in diesem Bereich:

XMAX=2176 - XMIN=1977 - YMAX=2122 - YMIN=1902

Das ergibt etwa das folgende Bild, wenn man alle möglichen Messwerte in ein Diagramm eintragen würde:

Nullpunktverschiebung

Jetzt muss nur noch der Nullpunkt des Kreises ermittelt werden:

Der Nullpunkt berechnet sich wie folgt:

X_NULLPUNKT = (XMAX-XMIN)/2 + XMIN

Y_NULLPUNKT = (YMAX-YMIN)/2 + YMIN

Schließlich wird der Mittelpunkt  des Messwert-Kreises rechnerisch so verschoben, dass er im Koordinatenursprung liegt. Soviel zur Theorie...

 


 

Nun habe ich versucht, die auf den Nullpunkt normierten (und gerundeten) Messwerte einmal in eine "Windrose" einzutragen:

Quadrantendarstellung

Dabei musste ich verblüfft feststellen, dass die X- und die Y-Achse ja eigentlich "vertauscht" sind. Die Angaben wie 100, 0 bzw. 0, 100 usw. sind die jeweiligen (gerundeten)  X/Y-Wertepaare.

Eigentlich ergibt sich kein ganz idealer Kreis sondern eher eine Ellipse.

Die Winkel in den 4 Quadranten werden nun über den Arcus-Tangens ermittelt. Das ist die Umkehrfunktion des Tangens.

Es gilt im rechtwinkligen Dreieck nämlich:

tan (a) = Gegenkathete / Ankathete -> a = atan(Gegenkathete / Ankathete)

Die atan-Funktion gehört zum Standard-ANSI-C und gilt im Bereich von -Pi/2 bis + Pi/2.

Ich habe nun für jeden Quadranten eine eigene Rechnung aufgemacht und auch die Fälle Y/X=0 und X/Y=0 abgefangen (Division durch NULL).

Heraus kommt schließlich ein Winkel von 0 bis 360 Grad. Das war ja das auch Ziel.

In dieser Programmversion verwende ich während einer Kalibrierung CompassCalibrate(void) gemessene Werte für X_MAX, X_MIN, Y_MAX und Y_MIN. Dazu muss sich der PRO-BOT mehrfach drehen, damit er den vollen Wertebereich der X- und Y-Messwerte scannen kann.

Programmlisting:

/*******************************************************************************
 Projektname:       KOMPASS_NIKO_CC.cprj
 Benötigte Libs´s:  IntFunc_lib.cc
 Routinen:          kompass.cc, PRO-BOT128C_Lib_V2.cc
 Autor:             Niko
 Datum:             3. 12. 2012

 Funktion:          Kompass starten und Winkel ausgeben
*******************************************************************************/

// Der MMC2120-Kompass-Chip besitzt ein Schreibregister, in das die Befehle ge-
// schrieben werden können sowie 5 Leseregister, in das der Inhalt des Status-
// registers und die Messwerte abgelegt werden:
// STATUS-Register - Adresse 0x00
// MSB des X-Messwertes - Adresse 0x01
// LSB des X-Messwertes - Adresse 0x02
// MSB des Y-Messwertes - Adresse 0x03
// LSB des Y-Messwertes - Adresse 0x04

// I2C-Adresse und Wartezeit definieren:
#define I2C_ADDR_KOMPASS 0x60 // fest eingestellt!
#define WAIT1 5 // Millisekunden
#define PI 3.1415

// Maximal/Minimalwerte für X und Y - durch Test ermittelt
#define X_MAX 1
#define X_MIN 5000
#define Y_MAX 1
#define Y_MIN 5000

// Register und Befehle des Kompass-Chips
#define WRITE_ADDRESS 0x00 // Adresse des Schreibregisters
#define READ_ADDRESS 0x00 // Basisadresse der 5 Leseregister
#define TAKE_MEASURE  0x01 // Befehl zum Start der Messung
#define SET_COIL 0x02 // Setzen der Magnetspule
#define RESET_COIL 0x04 // Löschen der Magnetspule
#define 12BIT_MASK 0x0FFF //Maskierung für 12-Bit-Werte

int XMAX, XMIN, YMAX, YMIN;

void main(void)
{
    PRO_BOT128_INIT();    //PRO-BOT128 Setup
    DELAY_MS(WAIT1);
    XMAX = X_MAX;
    XMIN = X_MIN;
    YMAX = Y_MAX;
    YMIN = Y_MIN;

    CompassResetSetCoils();
    CompassCalibrate();


    while(1)
    {
    int Winkel;
    Winkel=CompassGetDegree();
    Msg_WriteText("Winkel:");
    Msg_WriteInt(Winkel);
    Msg_WriteChar(13);
    }
}

void CompassResetSetCoils (void)
{
    // RESET / SET Coils
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS);
    I2C_Write(WRITE_ADDRESS); // als WAKE-UP
    I2C_Write(RESET_COIL);
    I2C_Stop();
    DELAY_MS(WAIT1);
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS);
    I2C_Write(WRITE_ADDRESS); // als WAKE-UP
    I2C_Write(SET_COIL);
    I2C_Stop();
    DELAY_MS(WAIT1);
}

int CompassGetDegree(void)
{
    byte MSB_X, LSB_X, MSB_Y, LSB_Y; //Rohdaten / Messwerte 8 Bit
    int X_VALUE, Y_VALUE; // 12-Bit-Messwerte
    int X_OFFSET, Y_OFFSET; // Nullpunkt des Messkreises
    float X_NORMIERT, Y_NORMIERT; // Auf Nullpunkt normierte Messwerte
     int mygrad; // fertig berechneter Winkel 0...360 Grad

    // Messung starten und Lesen der 5 Byte des Kompassmoduls
    // Die Reihenfolge der Befehle wird im Datenblatt so verlangt:
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS);
    I2C_Write(WRITE_ADDRESS); // Basisadresse der Kompass-Register
    I2C_Write(TAKE_MEASURE); // Messung starten
    I2C_Stop();
    DELAY_MS(WAIT1);  // WAIT erforderlich für die Messung!
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS);
    // STATUS-REGISTER auf Basisadresse soll nicht gelesen werden:
    I2C_Write(READ_ADDRESS+1); // Setze Leseadresse+1 der Kompass-Lese-Register
    I2C_Stop();
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS+1); // Lese-Adresse = I2C-Adresse + 1!
    // Ersten Wert auslesn
    MSB_X = I2C_Read_ACK();
    // Zweiten Wert auslesen
    LSB_X =I2C_Read_ACK();
    // Dritten Wert auslesen
    MSB_Y = I2C_Read_ACK();
     // Vierten Wert auslesen
    LSB_Y =I2C_Read_NACK(); //Hier KEIN ACKNOWLEDGE!
    I2C_Stop();
    DELAY_MS(WAIT1);

    // Messwerte zu 12-Bit-Zahl zusammensetzen
    X_VALUE = MSB_X*256 + LSB_X;
    X_VALUE = X_VALUE & 12BIT_MASK;
    Y_VALUE = MSB_Y*256 + LSB_Y;
    Y_VALUE = Y_VALUE & 12BIT_MASK;

    // Nullpunkt des Meswswertkreises ermittel:
    X_OFFSET = (XMAX-XMIN)/2+XMIN;
    Y_OFFSET = (YMAX-YMIN)/2+YMIN;
    X_NORMIERT = X_VALUE-X_OFFSET;
    Y_NORMIERT = Y_VALUE-Y_OFFSET;

     Msg_WriteText("X_NORMIERT:");
    Msg_WriteInt(X_NORMIERT);
     Msg_WriteText("-Y_NORMIERT:");
    Msg_WriteInt(Y_NORMIERT);
    Msg_WriteChar(13);

    // Fallunterscheidung nach Quadranten:
    // NORD_OST
    if ((X_NORMIERT<0)&&(Y_NORMIERT<0))
        {
        mygrad = atan(Y_NORMIERT/X_NORMIERT) * 180 / PI;
        }
    // SÜD_OST
    if ((X_NORMIERT>0)&&(Y_NORMIERT<0))
        {
        mygrad = 90+atan(-X_NORMIERT/Y_NORMIERT) * 180 / PI;
        }
    // SÜD_WEST
    if ((X_NORMIERT>0)&&(Y_NORMIERT>0))
        {
        mygrad = 180+atan(Y_NORMIERT/X_NORMIERT) * 180 / PI;
        }
    // NORD_WEST
     if ((X_NORMIERT<0)&&(Y_NORMIERT>0))
        {
        mygrad = 270+atan(-X_NORMIERT/Y_NORMIERT) * 180 / PI;
        }
      if ((X_NORMIERT==0)&&(Y_NORMIERT<0))
        {
        mygrad = 90;
        }
       if ((Y_NORMIERT==0)&&(X_NORMIERT>0))
        {
        mygrad = 180;
        }
      if ((X_NORMIERT==0)&&(Y_NORMIERT>0))
        {
        mygrad = 270;
        }
      if ((Y_NORMIERT==0)&&(X_NORMIERT<0))
        {
        mygrad = 0;
        }
        return mygrad;
}

void CompassCalibrate(void)
{
    byte MSB_X, LSB_X, MSB_Y, LSB_Y; //Rohdaten / Messwerte 8 Bit
    int X_VALUE, Y_VALUE; // 12-Bit-Messwerte
    DRIVE_ON();//ENABLE for Motore
    MOTOR_DIR(1,0); //Richtung "Vorwärts" und "Rückwärts"
    MOTOR_POWER(200,200);
    int i;
    for (i=0; i<1000; i++)
    {
    // PRO-BOT128 dreht ein kleines Stück und nimmt Messwerte
    // Messung starten und Lesen der 5 Byte des Kompassmoduls
    // Die Reihenfolge der Befehle wird im Datenblatt so verlangt:
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS);
    I2C_Write(WRITE_ADDRESS); // Basisadresse der Kompass-Register
    I2C_Write(TAKE_MEASURE); // Messung starten
    I2C_Stop();
    DELAY_MS(WAIT1);  // WAIT erforderlich für die Messung!
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS);
    // STATUS-REGISTER auf Basisadresse soll nicht gelesen werden:
    I2C_Write(READ_ADDRESS+1); // Setze Leseadresse+1 der Kompass-Lese-Register
    I2C_Stop();
    I2C_Start();
    I2C_Write(I2C_ADDR_KOMPASS+1); // Lese-Adresse = I2C-Adresse + 1!
    // Ersten Wert auslesn
    MSB_X = I2C_Read_ACK();
    // Zweiten Wert auslesen
    LSB_X =I2C_Read_ACK();
    // Dritten Wert auslesen
    MSB_Y = I2C_Read_ACK();
     // Vierten Wert auslesen
    LSB_Y =I2C_Read_NACK(); //Hier KEIN ACKNOWLEDGE!
    I2C_Stop();
    DELAY_MS(WAIT1);

    // Messwerte zu 12-Bit-Zahl zusammensetzen
    X_VALUE = MSB_X*256 + LSB_X;
    X_VALUE = X_VALUE & 12BIT_MASK;
    Y_VALUE = MSB_Y*256 + LSB_Y;
    Y_VALUE = Y_VALUE & 12BIT_MASK;

    if (X_VALUE > XMAX) XMAX = X_VALUE;
    if (Y_VALUE > YMAX) YMAX = Y_VALUE;
    if (X_VALUE < XMIN) XMIN = X_VALUE;
    if (Y_VALUE < YMIN) YMIN = Y_VALUE;

    Msg_WriteText("XMAX:");
    Msg_WriteInt(XMAX);
    Msg_WriteText("YMAX:");
    Msg_WriteInt(YMAX);
    Msg_WriteText("XMIN:");
    Msg_WriteInt(XMIN);
    Msg_WriteText("YMIN:");
    Msg_WriteInt(YMIN);
    Msg_WriteChar(13);

    }
    DRIVE_OFF();//Motoren aus

}


Buchempfehlung:

Buchempfehlung

Externer Link PRO-BOT128 selbst bauen und erfolgreich einsetzen

.

 

Letzte Änderung:
March 23. 2023 21:04:40
«    top    »