/*******************************************
*   Sample sketch that configures an HMC5883L 3 axis
*   magnetometer to continuous mode and reads back
*   the three axis of data.
*******************************************/

#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>

const AP_HAL_AVR::HAL_AVR& hal = AP_HAL_AVR_APM2;

#define HMC5883L  0x1E

void setup() {
    hal.uart0->begin(115200);
    hal.uart0->printf_P(PSTR("Initializing HMC5883L at address %x\r\n"),
                                HMC5883L);

    // configure device for continuous mode
    hal.i2c->begin();
    hal.i2c->setTimeout(100);

    uint8_t stat = hal.i2c->writeRegister(HMC5883L,0x02,0x00);
    if (stat == 0) {
        hal.uart0->printf_P(PSTR("successful init\r\n"));
    } else {
        hal.uart0->printf_P(PSTR("failed init: return status %d\r\n"),
                (int)stat);
        for(;;);
    }
}

void loop() {
    uint8_t data[6];
    //read 6 bytes (x,y,z) from the device
    uint8_t stat = hal.i2c->readRegisters(HMC5883L,0x03,6, data);

    if (stat == 0){
        int x, y, z;
        x = data[0] << 8;
        x |= data[1];
        y = data[2] << 8;
        y |= data[3];
        z = data[4] << 8;
        z |= data[5];
        hal.uart0->printf_P(PSTR("x: %d y: %d z: %d \r\n"), x, y, z);
    } else { 
        hal.uart0->printf_P(PSTR("i2c error: status %d\r\n"), (int)stat);
    }
}


extern "C" {
int main (void) {
    hal.init(NULL);
    setup();
    for(;;) loop();
    return 0;
}
}