Diff: QMC5883L.cpp

Revision:

5:d046009bc7c3

Parent:

4:2620ae5391a6

--- a/QMC5883L.cpp	Thu Oct 11 17:46:43 2018 +0000
+++ b/QMC5883L.cpp	Tue Nov 13 14:37:19 2018 +0000
@@ -55,6 +55,9 @@
     char data_out[2];
     data_out[0]=QMC5883L_reg;
     data_out[1]=QMC5883L_data;
+    #ifdef DEBUG
+        printf("WRITE: %d, %x, %x\r\n", QMC5883L_ADDRESS, data_out[0], data_out[1]);
+    #endif
     this->QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
 }
 
@@ -62,8 +65,14 @@
 {
     char data_out[1], data_in[1];
     data_out[0] = QMC5883L_reg;
+    #ifdef DEBUG
+        printf("RWRITE: %d, %x\r\n", QMC5883L_ADDRESS, data_out[0]);
+    #endif
     this->QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 1, 1);
     this->QMC5883L_i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
+    #ifdef DEBUG
+        printf("Read: %d, %x\r\n", QMC5883L_ADDRESS, data_in);
+    #endif
     return (data_in[0]);
 }
 
@@ -74,12 +83,34 @@
 
 void QMC5883L::init()
 {   
-    setMagRange(MagScale_8G);
-    QMC5883L_WriteByte(CONTROL_A, 0x0D | MagScale_8G);  // Range: 8G, ODR: 200 Hz, mode:Continuous-Measurement
+    changeState(osr512, MagScale_8G, odr_200hz, mode_continuous);
     QMC5883L_WriteByte(SET_RESET, 0x01);
     wait_ms(10);
 }
 
+void QMC5883L::changeState(OSR osr, MagScale rng, ODR odr, Mode mode)
+{
+    char tmp = ((osr & 0x3) << 6) | ((rng & 0x3) << 4) | ((odr & 0x3) << 2) | (mode & 0x3);
+    QMC5883L_WriteByte(CONTROL_A, tmp);
+}
+
+void QMC5883L::standby()
+{
+    // Standby mode
+    changeState(osr512, MagScale_2G, odr_1Ohz, mode_standby);
+    wait_ms(10);
+}
+
+void QMC5883L::reset()
+{
+    // Reset
+    QMC5883L_WriteByte(CONTROL_B, 0x80);
+    wait_ms(10);
+    
+    // Config de base
+    init();
+}
+
 int16_t QMC5883L::getMagXvalue()
 {
     uint8_t LoByte, HiByte;