Mems sensor
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