File content as of revision 74:68c536a821ce:

#include "QMC5883L.h"
 
QMC5883L::QMC5883L(I2C& i2c): i2c(i2c)
{
    init();
}
 
void QMC5883L::readMag()
{
    getMagValue(OUT_Y_LSB, OUT_Y_MSB, m_mag_val[0]); // x and y are swapped
    getMagValue(OUT_X_LSB, OUT_X_MSB, m_mag_val[1]);
    getMagValue(OUT_Z_LSB, OUT_Z_MSB, m_mag_val[2]);
}
 
float QMC5883L::magX() 
{
    return m_scale * static_cast<float>(m_mag_val[0]);
}
 
float QMC5883L::magY() 
{
    return m_scale * static_cast<float>(m_mag_val[1]);
}
 
float QMC5883L::magZ() 
{
    return m_scale * static_cast<float>(m_mag_val[2]);
}
 
void QMC5883L::init()
{   
    // 31.05.2022, based on experiment so it fits the former mag scaling (norm approx. 0.31)
    m_scale = 1.97e-4f;
    
    static bool ok;
    static uint8_t cntr;
    
    cntr = 0;
    while (true) {
        //ok = WriteByte(CONTROL_A, 0x59);  // OSR: 256, RNG: 8G, ODR: 100 Hz, MODE: cont.
        ok = WriteByte(CONTROL_A, 0x1D);  // OSR: 512, RNG: 8G, ODR: 200 Hz, MODE: cont. (betaflight and inav)
        cntr++;
        if (ok || cntr == 5) {
            break;
        }
    }
    if (!ok) printf("   QMC5883L initialisation failed");
    
    cntr = 0;
    while (true) {
        ok = WriteByte(SET_RESET, 0x01);
        cntr++;
        if (ok || cntr == 5) {
            break;
        }
    }
    if (!ok) printf("   QMC5883L soft reset not successful");
    
    thread_sleep_for(10);
}
 
void QMC5883L::getMagValue(const uint8_t& OUT_LSB, const uint8_t& OUT_MSB, int16_t& retval)
{
    static uint8_t LoByte, HiByte;
    bool ok = ReadByte(OUT_LSB, LoByte);
    if (ok) {
        ok = ReadByte(OUT_MSB, HiByte);
        if (ok) {
            retval = (HiByte<<8) | LoByte;
        }
    }
}
 
bool QMC5883L::ReadByte(const uint8_t& reg, uint8_t& data)
{
    static char data_out[1], data_in[1];
    data_out[0] = reg;
    bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 1, 0);
    if (!nack) {
        nack = i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
        if (!nack) {
            data =  (data_in[0]);
        }
    }
    return !nack;
}
 
bool QMC5883L::WriteByte(const uint8_t& reg, const uint8_t& data)
{
    static char data_out[2];
    data_out[0] = reg;
    data_out[1] = data;
    bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
    return !nack;
}