Diff: QMC5883L.cpp

Revision:

43:f459f6efaf5c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QMC5883L.cpp	Tue May 31 17:51:34 2022 +0200
@@ -0,0 +1,79 @@
+#include "QMC5883L.h"
+ 
+QMC5883L::QMC5883L(I2C& i2c): i2c(i2c)
+{
+    init();
+}
+
+void QMC5883L::readMag()
+{
+    mag_val[0] = getMagXvalue();
+    mag_val[1] = getMagYvalue();
+    mag_val[2] = getMagZvalue();
+}
+
+float QMC5883L::magX() 
+{
+    return scale * static_cast<float>( mag_val[0] );
+}
+
+float QMC5883L::magY() 
+{
+    return scale * static_cast<float>( mag_val[1] );
+}
+
+float QMC5883L::magZ() 
+{
+    return scale * static_cast<float>( mag_val[2] );
+}
+
+void QMC5883L::init()
+{   
+    scale = 1.97e-4f; // 31.05.2022, based on experiment so it fits the former mag scaling (norm approx. 0.31)
+    QMC5883L_WriteByte(CONTROL_A, 0x1D);  // OSR: 512, RNG: 8G, ODR: 200 Hz, MODE: cont. (betaflight and inav)
+    //QMC5883L_WriteByte(CONTROL_A, 0x59);  // OSR: 256, RNG: 8G, ODR: 100 Hz, MODE: cont.
+    QMC5883L_WriteByte(SET_RESET, 0x01);
+    thread_sleep_for(10);
+}
+ 
+int16_t QMC5883L::getMagXvalue()
+{
+    uint8_t LoByte, HiByte;
+    LoByte = QMC5883L_ReadByte(OUT_Y_LSB); // x and y are swapped
+    HiByte = QMC5883L_ReadByte(OUT_Y_MSB);
+    return((HiByte<<8) | LoByte);
+}
+ 
+ 
+int16_t QMC5883L::getMagYvalue()
+{
+    uint8_t LoByte, HiByte;
+    LoByte = QMC5883L_ReadByte(OUT_X_LSB); // x and y are swapped
+    HiByte = QMC5883L_ReadByte(OUT_X_MSB);
+    return ((HiByte<<8) | LoByte);
+}
+ 
+int16_t QMC5883L::getMagZvalue()
+{
+    uint8_t LoByte, HiByte;
+    LoByte = QMC5883L_ReadByte(OUT_Z_LSB);
+    HiByte = QMC5883L_ReadByte(OUT_Z_MSB);
+    return ((HiByte<<8) | LoByte);
+}
+
+uint8_t QMC5883L::QMC5883L_ReadByte(uint8_t QMC5883L_reg)
+{
+    char data_out[1], data_in[1];
+    data_out[0] = QMC5883L_reg;
+    i2c.write(QMC5883L_ADDRESS, data_out, 1, 1);
+    i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
+    return (data_in[0]);
+}
+
+void QMC5883L::QMC5883L_WriteByte(uint8_t QMC5883L_reg, uint8_t QMC5883L_data)
+{
+    char data_out[2];
+    data_out[0] = QMC5883L_reg;
+    data_out[1] = QMC5883L_data;
+    i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
+}