File content as of revision 13:bc20290eaff2:

#include "Magnetometer.h"
#define DEBUG "BMX055-Mag"
#include "Logger.h"

#include "Vector3.h"

Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), sum(0, 0, 0), hardIron(0,0,0), numSamples(0)
{
    if (powerOn()) {
        readCalibrationData();
        INFO("Bosch Sensortec BMX055-Magneto found");
        powerOff();
    } else {
        WARN("Bosch Sensortec BMX055-Magneto not found");
    }
}

bool Magnetometer::powerOn()
{
    write_reg(0x4B, 0x82); // softreset with power off
    wait_ms(3); // page 18
    write_reg(0x4B, 0x83); // softreset with power on, this causes full POR. page 134.
    wait_ms(3);
    return read_reg(0x40) == 0x32; // verify chip ID
}

void Magnetometer::powerOff()
{
    write_reg(0x4B, 0x82); // softreset and stay powered off
    LOG("powered off");
}

bool Magnetometer::performSelfTest()
{
    write_reg(0x4C, 0x06); // go to sleep mode
    write_reg(0x4C, 0x07); // start self test
    wait_ms(50);

    const bool done = (read_reg(0x4C) & 0x01) == 0;
    const bool x_passed = read_reg(0x42) & 0x01;
    const bool y_passed = read_reg(0x44) & 0x01;
    const bool z_passed = read_reg(0x46) & 0x01;

    INFO("Self test done=%s x=%s y=%s z=%s", done?"pass":"fail", x_passed?"pass":"fail", y_passed?"pass":"fail", z_passed?"pass":"fail");

    return done && x_passed && y_passed && z_passed;
}

void Magnetometer::start()
{
    // configure for "high accuracy preset" pg. 122
    write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis
    write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis
    write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start
}

void Magnetometer::stop()
{
    write_reg(0x4C, 0x06); // 10 Hz but sleep mode
}

Vector3 Magnetometer::read()
{
    // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790
    uint8_t buffer[8];

    for (size_t i = 0; i < sizeof buffer; i++)
        buffer[i] = read_reg(0x42 + i);

    //read_reg(0x40, buffer, 4);

    // Datasheet is wrong, BMX055 magneto x and y axis are interchanged and y axis is inverted !!!
    const int16_t mdata_y = *(reinterpret_cast<const int16_t*>(buffer + 0)) / -8;
    const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8;
    const int16_t mdata_z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 2;
    const uint16_t data_r = *(reinterpret_cast<const uint16_t*>(buffer + 6)) / 4;
    int16_t temp; // temporary
    Vector3 magData;

    // calculate temperature compensated 16-bit magnetic fields
    temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));

    magData.x = ((int16_t)((((int32_t)mdata_x) *
                            ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
                                 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
                               ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
                (((int16_t)dig_x1) << 3);

    temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
    magData.y = ((int16_t)((((int32_t)mdata_y) *
                            ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
                                 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
                               ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
                (((int16_t)dig_y1) << 3);

    magData.z = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) -
                  ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16))));

    return magData - Vector3(-720, 1481, -2830);
}

void Magnetometer::readCalibrationData()
{
    // trying to read in serial order of address
    dig_x1  = read_reg(0x5D);
    dig_y1  = read_reg(0x5E);
    dig_z4   = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8));
    dig_x2  = read_reg(0x64);
    dig_y2  = read_reg(0x65);
    dig_z2   = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8));
    dig_z1   = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8));
    dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8));
    dig_z3   = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8));
    dig_xy2 = read_reg(0x70);
    dig_xy1 = read_reg(0x71);
    LOG("calibration data loaded");
}