Diff: QMC5883L.cpp

Revision:

0:1d7c885bfcc6

Child:

1:40133ee060bb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QMC5883L.cpp	Sat Jan 26 21:39:07 2019 +0000
@@ -0,0 +1,113 @@
+#include "QMC5883L.h"
+
+QMC5883L::QMC5883L(PinName sda, PinName scl): QMC5883L_i2c(sda, scl)
+{
+}
+
+
+void QMC5883L::QMC5883L_w(char QMC5883L_reg, char QMC5883L_data)
+{
+    char data_out[2];
+    data_out[0]=QMC5883L_reg;
+    data_out[1]=QMC5883L_data;
+    QMC5883L_i2c.write(0x0D<<1, data_out, 2, 0);
+}
+
+
+char QMC5883L::QMC5883L_r(char QMC5883L_reg)
+{
+    char data_out[1], data_in[1];
+    data_out[0] = QMC5883L_reg;
+    QMC5883L_i2c.write(0x0D<<1, data_out, 1, 1);
+    QMC5883L_i2c.read(0x0D<<1, data_in, 1, 0);
+    return (data_in[0]);
+}
+
+
+void QMC5883L::setRange(int range) // vrijednost 2 za 2G, vrijednost 8 za 8G
+{
+    switch(range)
+    {
+        case 2:
+            range_data = 0x00; 
+            mRes = 1.0/12000;
+            break;
+        case 8:
+            range_data = 0x01;
+            mRes = 1.0/3000;
+            break;
+    } 
+}
+
+
+void QMC5883L::init()
+{   
+    QMC5883L_w (CONTROL_A, 0x0D | range_data); // 
+    QMC5883L_w (SET_RESET, 0x01);
+    
+    wait_ms(10);
+}
+
+
+short QMC5883L::getXvalue()
+{
+    char eLSB, eMSB;
+    eLSB = QMC5883L_r(OUT_X_LSB); // očitaj LSB x
+    eMSB = QMC5883L_r(OUT_X_MSB); // očitaj MSB x
+    return((eMSB<<8) | eLSB);
+}
+
+
+short QMC5883L::getYvalue()
+{
+    char eLSB, eMSB;
+    eLSB = QMC5883L_r(OUT_Y_LSB); // očitaj LSB y
+    eMSB = QMC5883L_r(OUT_Y_MSB); // očitaj MSB y
+    return ((eMSB<<8) | eLSB);
+}
+
+
+short QMC5883L::getZvalue()
+{
+   char eLSB, eMSB;
+    eLSB = QMC5883L_r(OUT_Z_LSB); // očitaj LSB z
+    eMSB = QMC5883L_r(OUT_Z_MSB); // očitaj MSB z
+    return ((eMSB<<8) | eLSB);
+}
+
+
+float QMC5883L::getTemp()
+{
+    char eLSB, eMSB;
+    float Temp_temp;
+    short temp_data;
+    eLSB = QMC5883L_r(TEMP_LSB); // očitaj LSB temp
+    eMSB = QMC5883L_r(TEMP_MSB); // očitaj MSB temp
+    temp_data = ((eMSB<<8) | eLSB);
+    Temp_temp = (temp_data /100.0)+ 40;     // relativna temp + sobna
+    return Temp_temp;
+}
+
+
+double QMC5883L::getHeading()
+{
+    double x,y,z;
+    double heading;
+    
+    x = QMC5883L::getXvalue() *(100*mRes);  // iz Gaussa u uT * rezolucija
+    y = QMC5883L::getYvalue() *(100*mRes);
+    z = QMC5883L::getZvalue() *(100*mRes);
+    
+    heading = ( atan2(y,x) + 0.069813 );   // magnetska deklinacija na Voltinom naselju u radijanima (4°4' positive, EAST - dodajemo)
+    
+    if(heading < 0){                       // korekcija za negativni predznak
+            heading = 2*pi + heading;
+            
+            }
+    else if (heading > 2*pi) {             // korekcija zbog dodatka mag. deklinacije - overflow
+            heading = heading - 2*pi;
+            }
+    heading = heading * 180 / pi;          // pretvorba iz radijana u stupnjeve
+    
+    return heading;   
+}