QMC5883L.cpp

00001 #include "QMC5883L.h"
00002  
00003 QMC5883L::QMC5883L(I2C& i2c): i2c(i2c)
00004 {
00005     init();
00006 }
00007  
00008 void QMC5883L::readMag()
00009 {
00010     getMagValue(OUT_Y_LSB, OUT_Y_MSB, m_mag_val[0]); // x and y are swapped
00011     getMagValue(OUT_X_LSB, OUT_X_MSB, m_mag_val[1]);
00012     getMagValue(OUT_Z_LSB, OUT_Z_MSB, m_mag_val[2]);
00013 }
00014  
00015 float QMC5883L::magX() 
00016 {
00017     return m_scale * static_cast<float>(m_mag_val[0]);
00018 }
00019  
00020 float QMC5883L::magY() 
00021 {
00022     return m_scale * static_cast<float>(m_mag_val[1]);
00023 }
00024  
00025 float QMC5883L::magZ() 
00026 {
00027     return m_scale * static_cast<float>(m_mag_val[2]);
00028 }
00029  
00030 void QMC5883L::init()
00031 {   
00032     // 31.05.2022, based on experiment so it fits the former mag scaling (norm approx. 0.31)
00033     m_scale = 1.97e-4f;
00034     
00035     static bool ok;
00036     static uint8_t cntr;
00037     
00038     cntr = 0;
00039     while (true) {
00040         //ok = WriteByte(CONTROL_A, 0x59);  // OSR: 256, RNG: 8G, ODR: 100 Hz, MODE: cont.
00041         ok = WriteByte(CONTROL_A, 0x1D);  // OSR: 512, RNG: 8G, ODR: 200 Hz, MODE: cont. (betaflight and inav)
00042         cntr++;
00043         if (ok || cntr == 5) {
00044             break;
00045         }
00046     }
00047     if (!ok) printf("   QMC5883L initialisation failed");
00048     
00049     cntr = 0;
00050     while (true) {
00051         ok = WriteByte(SET_RESET, 0x01);
00052         cntr++;
00053         if (ok || cntr == 5) {
00054             break;
00055         }
00056     }
00057     if (!ok) printf("   QMC5883L soft reset not successful");
00058     
00059     thread_sleep_for(10);
00060 }
00061  
00062 void QMC5883L::getMagValue(const uint8_t& OUT_LSB, const uint8_t& OUT_MSB, int16_t& retval)
00063 {
00064     static uint8_t LoByte, HiByte;
00065     bool ok = ReadByte(OUT_LSB, LoByte);
00066     if (ok) {
00067         ok = ReadByte(OUT_MSB, HiByte);
00068         if (ok) {
00069             retval = (HiByte<<8) | LoByte;
00070         }
00071     }
00072 }
00073  
00074 bool QMC5883L::ReadByte(const uint8_t& reg, uint8_t& data)
00075 {
00076     static char data_out[1], data_in[1];
00077     data_out[0] = reg;
00078     bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 1, 0);
00079     if (!nack) {
00080         nack = i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
00081         if (!nack) {
00082             data =  (data_in[0]);
00083         }
00084     }
00085     return !nack;
00086 }
00087  
00088 bool QMC5883L::WriteByte(const uint8_t& reg, const uint8_t& data)
00089 {
00090     static char data_out[2];
00091     data_out[0] = reg;
00092     data_out[1] = data;
00093     bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
00094     return !nack;
00095 }