Diff: MagTrix.cpp

Revision:

0:fb19c8f7ca8d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MagTrix.cpp	Thu Mar 01 11:35:10 2018 +0000
@@ -0,0 +1,133 @@
+#include "MagTrix.h"
+
+MagTrix::MagTrix(I2C * i2c) :
+    _i2c(i2c),
+    _device1(0x19, i2c),
+    _device2(0x1B, i2c),
+    _device3(0x1D, i2c),
+    _device4(0x1F, i2c)
+{
+    
+}
+
+void MagTrix::init(char * receiveBuffer)
+{
+    BusOut mux(D8, D9);
+    
+    mux = 0x00;
+        
+    _device1.SB(receiveBuffer, 0xF, 0);
+    _device2.SB(receiveBuffer, 0xF, 0);
+    _device3.SB(receiveBuffer, 0xF, 0);
+    _device4.SB(receiveBuffer, 0xF, 0);
+    
+    mux = 0x01;
+    
+    _device1.SB(receiveBuffer, 0xF, 0);
+    _device2.SB(receiveBuffer, 0xF, 0);
+    _device3.SB(receiveBuffer, 0xF, 0);
+    _device4.SB(receiveBuffer, 0xF, 0);
+
+    mux = 0x02;
+    
+    _device1.SB(receiveBuffer, 0xF, 0);
+    _device2.SB(receiveBuffer, 0xF, 0);
+    _device3.SB(receiveBuffer, 0xF, 0);
+    _device4.SB(receiveBuffer, 0xF, 0);
+    
+    mux = 0x03;
+    
+    _device1.SB(receiveBuffer, 0xF, 0);
+    _device2.SB(receiveBuffer, 0xF, 0);
+    _device3.SB(receiveBuffer, 0xF, 0);
+    _device4.SB(receiveBuffer, 0xF, 0); 
+}
+
+void MagTrix::read(char device, char * receiveBuffer, uint16_t * dataBuffer)
+{
+    BusOut mux(D8,D9);
+    
+    switch (device) {
+        
+        case (0x00) :
+            mux = 0x0;
+            _device1.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x01) :
+            mux = 0x0;
+            _device2.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x02) :
+            mux = 0x0;
+            _device3.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x03) :
+            mux = 0x0;
+            _device4.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x04) :
+            mux = 0x1;
+            _device1.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x05) :
+            mux = 0x1;
+            _device2.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x06) :
+            mux = 0x1;
+            _device3.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x07) :
+            mux = 0x1;
+            _device4.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x08) :
+            mux = 0x2;
+            _device1.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x09) :
+            mux = 0x2;
+            _device2.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x0A) :
+            mux = 0x2;
+            _device3.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x0B) :
+            mux = 0x2;
+            _device4.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x0C) :
+            mux = 0x3;
+            _device1.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x0D) :
+            mux = 0x3;
+            _device2.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x0E) :
+            mux = 0x3;
+            _device3.RM(receiveBuffer, 0xF, 0);
+            break;
+        case (0x0F) :
+            mux = 0x3;
+            _device4.RM(receiveBuffer, 0xF, 0);
+            break;
+        default:
+            break;
+    }
+    
+    // Combine MSB and LSB to aquire raw decimal values
+    int16_t xM = receiveBuffer[3] * 256 + receiveBuffer[4];
+    int16_t yM = receiveBuffer[5] * 256 + receiveBuffer[6];
+    int16_t zM = receiveBuffer[7] * 256 + receiveBuffer[8];
+    
+    // Gain values of 0
+    static int16_t xC = 0;
+    static int16_t yC = 20;
+    static int16_t zC = 400;
+
+    dataBuffer[0] = xM * 0.00805 - xC;
+    dataBuffer[1] = yM * 0.00805 - yC;
+    dataBuffer[2] = zM * 0.02936 - zC;
+}