Max Houghton
/
MagTrix
MagTrix Class
Diff: MagTrix.cpp
- Revision:
- 0:fb19c8f7ca8d
diff -r 000000000000 -r fb19c8f7ca8d MagTrix.cpp --- /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; +}