Diff: QMC5883L.cpp

Revision:

47:c74d09a252d4

Child:

49:76fcaffb92ef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QMC5883L.cpp	Thu Jun 02 07:43:14 2022 +0000
@@ -0,0 +1,95 @@
+#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;
+}