v61_0
Diff: LSM6DS33_GR1.cpp
- Revision:
- 6:798481567563
- Child:
- 7:164a2086348d
diff -r 45c2ff2955d4 -r 798481567563 LSM6DS33_GR1.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM6DS33_GR1.cpp Tue Jul 06 07:12:44 2021 +0000 @@ -0,0 +1,547 @@ +#include "LSM6DS33_GR1.h" + +LSM6DS33::LSM6DS33(PinName sda, PinName scl, uint8_t xgAddr) : i2c(sda, scl) +{ + // xgAddress will store the 7-bit I2C address, if using I2C. + xgAddress = xgAddr; + +// init gyro offset + gx_off=0; + gy_off=0; + gz_off=0; + gxol=0; + gxoh=0; + gyol=0; + gyoh=0; + gzol=0; + gzoh=0; +} + +uint16_t LSM6DS33::begin(gyro_scale gScl, accel_scale aScl, + gyro_odr gODR, accel_odr aODR) +{ + + // Store the given scales in class variables. These scale variables + // are used throughout to calculate the actual g's, DPS,and Gs's. + gScale = gScl; + aScale = aScl; + + // Once we have the scale values, we can calculate the resolution + // of each sensor. That's what these functions are for. One for each sensor + calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable + calcaRes(); // Calculate g / ADC tick, stored in aRes variable + + + // To verify communication, we can read from the WHO_AM_I register of + // each device. Store those in a variable so we can return them. + // The start of the addresses we want to read from + char cmd[2] = { + WHO_AM_I_REG, + 0 + }; + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, cmd, 1, true); + // Read in all the 8 bits of data + i2c.read(xgAddress, cmd+1, 1); + uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I + + // Gyro initialization stuff: + initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc. + setGyroODR(gODR); // Set the gyro output data rate and bandwidth. + setGyroScale(gScale); // Set the gyro range + + // Accelerometer initialization stuff: + initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc. + setAccelODR(aODR); // Set the accel data rate. + setAccelScale(aScale); // Set the accel range. + + //set high res timestamp where LSB is 25us + cmd[0] = WAKE_UP_DUR; + cmd[1] = 0x10; + i2c.write(xgAddress, cmd, 2); + + + // Once everything is initialized, return the WHO_AM_I registers we read: + return xgTest; +} + +void LSM6DS33::initGyro() +{ + char cmd[2] = { + CTRL2_G, + gScale | G_ODR_104 + }; + + // Write the data to the gyro control registers + i2c.write(xgAddress, cmd, 2); +} + +void LSM6DS33::initAccel() +{ + char cmd[4] = { + CTRL1_XL, + 0x30 + }; + + // Write the data to the accel control registers + i2c.write(xgAddress, cmd, 2); +} + +void LSM6DS33::initIntr() +{ + +} +//modif gr1 +void LSM6DS33::readAllraw() +{ + // The data we are going to read from the temp/gyr/acc/timestamp + //char data[14];//from 0x20 to 0x42 + + char data[14]; + char tsdata[3]; + + i2c.start(); + i2c.write(xgAddress); + i2c.write(OUT_TEMP_L); + i2c.start(); + i2c.write(xgAddress | 0x1); + for(int i =0; i<13; i++) { + data[i]=i2c.read(1); + } + data[13]=i2c.read(0); + i2c.stop(); + + // Temperature is a 12-bit signed integer + //temperature_raw = data[0] | (data[1] << 8); + gxl = data[2] ; + gxh =data[3] ; + gyl = data[4] ; + gyh= data[5] ; + gzl = data[6] ; + gzh=data[7] ; + axl= data[8] ; + axh=data[9] ; + ayl = data[10]; + ayh=data[11] ; + azl = data[12] ; + azh=data[13] ; + + //i2c.start(); + // i2c.write(xgAddress); + // i2c.write(TIMESTAMP0_REG); + // i2c.start(); + // i2c.write(xgAddress | 0x1); + // for(int i =0; i<3; i++) { + // tsdata[i]=i2c.read(1); + // } + // tsdata[3]=i2c.read(0); + //i2c.stop(); + + // time_raw = tsdata[0] | (tsdata[1] << 8) | (tsdata[2] << 16); + + + // temperature_c = (float)temperature_raw / 16.0 + 25.0; + // gx = gx_raw * gRes; + // gy = gy_raw * gRes; + // gz = gz_raw * gRes; + // ax = ax_raw * aRes; + // ay = ay_raw * aRes; + // az = az_raw * aRes; + // time = time_raw*(0.000025); + + +} +//fin modif gr1 +void LSM6DS33::readAll() +{ + // The data we are going to read from the temp/gyr/acc/timestamp + //char data[14];//from 0x20 to 0x42 + + char data[14]; + char tsdata[3]; + + i2c.start(); + i2c.write(xgAddress); + i2c.write(OUT_TEMP_L); + i2c.start(); + i2c.write(xgAddress | 0x1); + for(int i =0; i<13; i++) { + data[i]=i2c.read(1); + } + data[13]=i2c.read(0); + i2c.stop(); + + // Temperature is a 12-bit signed integer + temperature_raw = data[0] | (data[1] << 8); + gx_raw = data[2] | (data[3] << 8); + gy_raw = data[4] | (data[5] << 8); + gz_raw = data[6] | (data[7] << 8); + ax_raw = data[8] | (data[9] << 8); + ay_raw = data[10] | (data[11] << 8); + az_raw = data[12] | (data[13] << 8); + + i2c.start(); + i2c.write(xgAddress); + i2c.write(TIMESTAMP0_REG); + i2c.start(); + i2c.write(xgAddress | 0x1); + for(int i =0; i<3; i++) { + tsdata[i]=i2c.read(1); + } + tsdata[3]=i2c.read(0); + i2c.stop(); + + time_raw = tsdata[0] | (tsdata[1] << 8) | (tsdata[2] << 16); + + + temperature_c = (float)temperature_raw / 16.0 + 25.0; + gx = gx_raw * gRes; + gy = gy_raw * gRes; + gz = gz_raw * gRes; + ax = ax_raw * aRes; + ay = ay_raw * aRes; + az = az_raw * aRes; + time = time_raw*(0.000025); + + +} + + +void LSM6DS33::readAccel() +{ + // The data we are going to read from the accel + char data[6]; + + // Set addresses + char subAddressXL = OUTX_L_XL; + char subAddressXH = OUTX_H_XL; + char subAddressYL = OUTY_L_XL; + char subAddressYH = OUTY_H_XL; + char subAddressZL = OUTZ_L_XL; + char subAddressZH = OUTZ_H_XL; + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, &subAddressXL, 1, true); + // Read in register containing the axes data and alocated to the correct index + i2c.read(xgAddress, data, 1); + + i2c.write(xgAddress, &subAddressXH, 1, true); + i2c.read(xgAddress, (data + 1), 1); + i2c.write(xgAddress, &subAddressYL, 1, true); + i2c.read(xgAddress, (data + 2), 1); + i2c.write(xgAddress, &subAddressYH, 1, true); + i2c.read(xgAddress, (data + 3), 1); + i2c.write(xgAddress, &subAddressZL, 1, true); + i2c.read(xgAddress, (data + 4), 1); + i2c.write(xgAddress, &subAddressZH, 1, true); + i2c.read(xgAddress, (data + 5), 1); + + // Reassemble the data and convert to g + ax_raw = data[0] | (data[1] << 8); + ay_raw = data[2] | (data[3] << 8); + az_raw = data[4] | (data[5] << 8); + ax = ax_raw * aRes; + ay = ay_raw * aRes; + az = az_raw * aRes; + //gr + axl= data[0] ; + axh=data[1] ; + ayl = data[2]; + ayh=data[3] ; + azl = data[4] ; + azh=data[5] ; +} + +void LSM6DS33::readIntr() +{ + char data[1]; + char subAddress = TAP_SRC; + + i2c.write(xgAddress, &subAddress, 1, true); + i2c.read(xgAddress, data, 1); + + intr = (float)data[0]; +} + +void LSM6DS33::readTemp() +{ + // The data we are going to read from the temp + char data[2]; + + // Set addresses + char subAddressL = OUT_TEMP_L; + char subAddressH = OUT_TEMP_H; + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, &subAddressL, 1, true); + // Read in register containing the temperature data and alocated to the correct index + i2c.read(xgAddress, data, 1); + + i2c.write(xgAddress, &subAddressH, 1, true); + i2c.read(xgAddress, (data + 1), 1); + + // Temperature is a 12-bit signed integer + temperature_raw = data[0] | (data[1] << 8); + + temperature_c = (float)temperature_raw / 16.0 + 25.0; + temperature_f = temperature_c * 1.8 + 32.0; +} + + +void LSM6DS33::readGyro() +{ + // The data we are going to read from the gyro + char data[6]; + + // Set addresses + char subAddressXL = OUTX_L_G; + char subAddressXH = OUTX_H_G; + char subAddressYL = OUTY_L_G; + char subAddressYH = OUTY_H_G; + char subAddressZL = OUTZ_L_G; + char subAddressZH = OUTZ_H_G; + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, &subAddressXL, 1, true); + // Read in register containing the axes data and alocated to the correct index + i2c.read(xgAddress, data, 1); + + i2c.write(xgAddress, &subAddressXH, 1, true); + i2c.read(xgAddress, (data + 1), 1); + i2c.write(xgAddress, &subAddressYL, 1, true); + i2c.read(xgAddress, (data + 2), 1); + i2c.write(xgAddress, &subAddressYH, 1, true); + i2c.read(xgAddress, (data + 3), 1); + i2c.write(xgAddress, &subAddressZL, 1, true); + i2c.read(xgAddress, (data + 4), 1); + i2c.write(xgAddress, &subAddressZH, 1, true); + i2c.read(xgAddress, (data + 5), 1); + + // Reassemble the data and convert to degrees/sec + gx_raw = data[0] | (data[1] << 8); + gy_raw = data[2] | (data[3] << 8); + gz_raw = data[4] | (data[5] << 8); + gx = gx_raw * gRes; + gy = gy_raw * gRes; + gz = gz_raw * gRes; + // gr + gxl = data[0] ; + gxh =data[1] ; + gyl = data[2] ; + gyh= data[3] ; + gzl = data[4] ; + gzh=data[5] ; +} + +void LSM6DS33::setGyroScale(gyro_scale gScl) +{ + // The start of the addresses we want to read from + char cmd[2] = { + CTRL2_G, + 0 + }; + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, cmd, 1, true); + // Read in all the 8 bits of data + i2c.read(xgAddress, cmd+1, 1); + + // Then mask out the gyro scale bits: + cmd[1] &= 0xFF^(0x7 << 1); //// << 2 au lieu de 3 + // Then shift in our new scale bits: + cmd[1] |= gScl << 1; //// << 0 au lieu de 3 + + // Write the gyroscale out to the gyro + i2c.write(xgAddress, cmd, 2); + + // We've updated the sensor, but we also need to update our class variables + // First update gScale: + gScale = gScl; + // Then calculate a new gRes, which relies on gScale being set correctly: + calcgRes(); +} + +void LSM6DS33::setAccelScale(accel_scale aScl) +{ + // The start of the addresses we want to read from + char cmd[2] = { + CTRL1_XL, + 0 + }; + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, cmd, 1, true); + // Read in all the 8 bits of data + i2c.read(xgAddress, cmd+1, 1); + + // Then mask out the accel scale bits: + cmd[1] &= 0xFF^(0x3 << 2); //// gr 2 au lieu de 3 mise a zero des bits 3 et 4 + // Then shift in our new scale bits: + cmd[1] |= aScl << 2; //// gr 2 au lieu de 3 + + // Write the accelscale out to the accel + i2c.write(xgAddress, cmd, 2); + + // We've updated the sensor, but we also need to update our class variables + // First update aScale: + aScale = aScl; + // Then calculate a new aRes, which relies on aScale being set correctly: + calcaRes(); +} + +void LSM6DS33::setGyroODR(gyro_odr gRate) +{ + // The start of the addresses we want to read from + char cmd[2] = { + CTRL2_G, + 0 + }; + + // Set low power based on ODR, else keep sensor on high performance + if(gRate == G_ODR_13_BW_0 | gRate == G_ODR_26_BW_2 | gRate == G_ODR_52_BW_16) { + char cmdLow[2] = { + CTRL7_G, + 1 + }; + + i2c.write(xgAddress, cmdLow, 2); + } else { + char cmdLow[2] = { + CTRL7_G, + 0 + }; + + i2c.write(xgAddress, cmdLow, 2); + } + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, cmd, 1, true); + // Read in all the 8 bits of data + i2c.read(xgAddress, cmd+1, 1); + + // Then mask out the gyro odr bits: + cmd[1] &= 0xFF^(0xF << 4); + // Then shift in our new odr bits: + cmd[1] |= gRate; + + // Write the gyroodr out to the gyro + i2c.write(xgAddress, cmd, 2); +} + +void LSM6DS33::setAccelODR(accel_odr aRate) +{ + // The start of the addresses we want to read from + char cmd[2] = { + CTRL1_XL, + 0 + }; + + // Set low power based on ODR, else keep sensor on high performance + if(aRate == A_ODR_13 | aRate == A_ODR_26 | aRate == A_ODR_52) { + char cmdLow[2] = { + CTRL6_C, + 1 + }; + + i2c.write(xgAddress, cmdLow, 2); + } else { + char cmdLow[2] = { + CTRL6_C, + 0 + }; + + i2c.write(xgAddress, cmdLow, 2); + } + + // Write the address we are going to read from and don't end the transaction + i2c.write(xgAddress, cmd, 1, true); + // Read in all the 8 bits of data + i2c.read(xgAddress, cmd+1, 1); + + // Then mask out the accel odr bits: + cmd[1] &= 0xFF^(0xF << 4); // gr erreur ?? + // Then shift in our new odr bits: + cmd[1] |= aRate << 4; // gr erreur + + // Write the accelodr out to the accel + i2c.write(xgAddress, cmd, 2); +} + +void LSM6DS33::calcgRes() +{ + // Possible gyro scales (and their register bit settings) are: + // 125 DPS , 245 DPS (00), 500 DPS (01), 2000 DPS (10). + switch (gScale) { + case G_SCALE_125DPS: + gRes = 125.0 / 32768.0; + break; + case G_SCALE_250DPS: + gRes = 250.0 / 32768.0; + break; + case G_SCALE_500DPS: + gRes = 500.0 / 32768.0; + break; + case G_SCALE_1000DPS: + gRes = 1000.0 / 32768.0; + break; + case G_SCALE_2000DPS: + gRes = 2000.0 / 32768.0; + break; + } +} + +void LSM6DS33::calcaRes() +{ + // Possible accelerometer scales (and their register bit settings) are: + // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). + switch (aScale) { + case A_SCALE_2G: + aRes = 2.0 / 32768.0; + break; + case A_SCALE_4G: + aRes = 4.0 / 32768.0; + break; + case A_SCALE_8G: + aRes = 8.0 / 32768.0; + break; + case A_SCALE_16G: + aRes = 16.0 / 32768.0; + break; + } +} +void LSM6DS33::calibration( int16_t iter) +{ + int32_t gxoll=0,gyoll=0,gzoll=0; + for(int ii=0; ii<iter; ii++) { + this->readGyro(); + gx_off=gx_off+gx; + gy_off=gy_off+gy; + gz_off=gz_off+gz; + // + + gxoll=gxoll+(int32_t)gx_raw; + gyoll=gyoll+(int32_t)gy_raw; + gzoll=gzoll+(int32_t)gz_raw; + + + //wait(0.01); + } + gx_off=gx_off/iter; + gy_off=gy_off/iter; + gz_off=gz_off/iter; + // + gxoll=gxoll/iter; + gyoll=gyoll/iter; + gzoll=gzoll/iter; + + // + gxol=(gxoll&0x00FF); + gxoh=(gxoll>>8); + gyol=(gyoll&0x00FF); + gyoh=(gyoll>>8); + gzol=(gzoll&0x00FF); + gzoh=(gzoll>>8); + +} +