Mems sensor
LSM9DS1.cpp
- Committer:
- Anunnaki
- Date:
- 2016-09-22
- Revision:
- 0:da46ed0f1363
- Child:
- 1:45447b012eea
File content as of revision 0:da46ed0f1363:
#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]; }