#include <Wire.h>

#define SDA_PIN 21
#define SCL_PIN 22

#define HMC5883L_ADDRESS 0x1E


void setup()
{
    Serial.begin(115200);

    delay(1000);

    Serial.println();
    Serial.println("================================");
    Serial.println(" TESTE HMC5883L");
    Serial.println("================================");

    Wire.begin(SDA_PIN, SCL_PIN);


    // Scanner I2C

    Serial.println();
    Serial.println("Procurando dispositivos I2C...");

    for (byte address = 1; address < 127; address++)
    {
        Wire.beginTransmission(address);

        byte error = Wire.endTransmission();

        if (error == 0)
        {
            Serial.print("Dispositivo encontrado: 0x");

            if (address < 16)
                Serial.print("0");

            Serial.println(address, HEX);
        }
    }

    Serial.println();
}


void loop()
{
    int16_t x;
    int16_t y;
    int16_t z;


    // Seleciona o primeiro registrador de dados

    Wire.beginTransmission(HMC5883L_ADDRESS);

    Wire.write(0x03);

    Wire.endTransmission(false);


    // Solicita 6 bytes

    Wire.requestFrom(HMC5883L_ADDRESS, 6);


    if (Wire.available() == 6)
    {
        // X

        x = Wire.read() << 8;
        x |= Wire.read();


        // Z

        z = Wire.read() << 8;
        z |= Wire.read();


        // Y

        y = Wire.read() << 8;
        y |= Wire.read();


        // =========================================
        // MOSTRA X, Y E Z
        // =========================================

        Serial.print("X = ");
        Serial.print(x);

        Serial.print("    Y = ");
        Serial.print(y);

        Serial.print("    Z = ");
        Serial.print(z);


        // =========================================
        // CALCULA O ANGULO
        // =========================================

        float angulo = atan2((float)y, (float)x);


        // Converte radianos para graus

        angulo = angulo * 180.0 / PI;


        // Corrige valores negativos

        if (angulo < 0)
        {
            angulo += 360.0;
        }


        // =========================================
        // MOSTRA O ANGULO
        // =========================================

        Serial.print("    Direcao = ");
        Serial.print(angulo, 1);
        Serial.println(" graus");
    }
    else
    {
        Serial.println("Erro na leitura do HMC5883L");
    }


    delay(500);
}