Diff: LSM9DS1.cpp

Revision:

0:da46ed0f1363

Child:

1:45447b012eea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS1.cpp	Thu Sep 22 14:13:04 2016 +0000
@@ -0,0 +1,134 @@
+#include "mbed.h"
+#include "LSM9DS1.h"
+#include "LSM9DS1_RegVals.h"
+
+LSM9DS1::LSM9DS1(I2C i2c) : _i2c(i2c){
+    startAcc();
+    startGyro();
+    startMag();
+} 
+
+void LSM9DS1::startMag(){
+    char data[2];
+
+    data[0] = (char)CTRL_REG1_M;          // Target register
+    data[1] = (char)0x7C;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+
+    data[0] = (char)CTRL_REG2_M;          // Target register
+    data[1] = (char)0x60;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0); 
+
+    data[0] = (char)CTRL_REG3_M;          // Target register
+    data[1] = (char)0x00;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+
+    data[0] = (char)CTRL_REG4_M;          // Target register
+    data[1] = (char)0x0C;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+
+    data[0] = (char)CTRL_REG5_M;          // Target register
+    data[1] = (char)0x00;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+}
+
+void LSM9DS1::readMag(float *results){
+    char results_[6];
+    float res_final[3];
+    char out_x_l_m = OUT_X_L_M;
+    
+    _i2c.write(TWI_MAG_ADDR, &out_x_l_m, 1, true);
+    _i2c.read(TWI_MAG_ADDR, results_, 6, 0);
+    res_final[0] = ((results_[1]<<8) | results_[0]);
+    res_final[1] = ((results_[3]<<8) | results_[2]);
+    res_final[2] = ((results_[5]<<8) | results_[4]);
+    
+    *(results) = res_final[0];
+    *(results + 1) = res_final[1];
+    *(results + 2) = res_final[2];
+}
+
+void LSM9DS1::startAcc(){
+    char data[2];
+    
+    data[0] = (char)CTRL_REG5_XL;          // Target register
+    data[1] = (char)0x38;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+    
+    data[0] = (char)CTRL_REG6_XL;          // Target register
+    data[1] = (char)0xC7;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+    
+}
+
+void LSM9DS1::readAcc(float *results){
+    char results_[6];
+    float res_final[3];
+    char out_x_l_xl = OUT_X_L_XL;
+    
+    _i2c.write(TWI_MAG_ADDR, &out_x_l_xl, 1, true);
+    _i2c.read(TWI_MAG_ADDR, results_, 6, 0);
+    res_final[0] = ((results_[1]<<8) | results_[0]);
+    res_final[1] = ((results_[3]<<8) | results_[2]);
+    res_final[2] = ((results_[5]<<8) | results_[4]);
+    
+    *(results) = res_final[0];
+    *(results + 1) = res_final[1];
+    *(results + 2) = res_final[2];
+}
+
+void LSM9DS1::startGyro(){
+    char data[2];
+    
+    // If GYRO is defines (gyro enabled)
+    #ifdef GYRO
+        data[0] = (char)CTRL_REG6_XL;                  // Target register
+        data[1] = (char)0xC7 & (char)0x1F;             // Data to write
+          = _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+         
+    #endif
+    
+    data[0] = (char)CTRL_REG1_G;          // Target register
+    data[1] = (char)0xC0;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+    
+    data[0] = (char)CTRL_REG2_G;          // Target register
+    data[1] = (char)0x00;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+    
+    data[0] = (char)CTRL_REG3_G;          // Target register
+    data[1] = (char)0x00;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+    
+    data[0] = (char)CTRL_REG4;            // Target register
+    data[1] = (char)0x3A;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+    
+    data[0] = (char)ORIENT_CFG_G;          // Target register
+    data[1] = (char)0x00;                 // Data to write
+    _i2c.write(TWI_MAG_ADDR, data, 0x02,0);
+     
+}
+
+void LSM9DS1::readGyro(float *results){
+    char results_[6];
+    float res_final[3];
+    char out_x_l_g = OUT_X_L_G;
+    
+    _i2c.write(TWI_MAG_ADDR, &out_x_l_g, 1, true);
+    _i2c.read(TWI_MAG_ADDR, results_, 6, 0);
+    res_final[0] = ((results_[1]<<8) | results_[0]);
+    res_final[1] = ((results_[3]<<8) | results_[2]);
+    res_final[2] = ((results_[5]<<8) | results_[4]);
+    
+    *(results) = res_final[0];
+    *(results + 1) = res_final[1];
+    *(results + 2) = res_final[2];
+}
\ No newline at end of file