lsm6ds0.cpp

00001 // Library for the STMicroelectronics LSM6DS0 3D accelerometer and 3D gyroscope
00002 
00003 #include "lsm6ds0.h"
00004 #include "functions.h"
00005 
00006 static Serial pc(SERIAL_TX, SERIAL_RX);
00007 
00008 void LSM6DS0::setFIFO(LSM6DS0_FIFO_mode_t work_mode)
00009 {
00010     char data_write[1];
00011     
00012     data_write[0] = work_mode;                          // set FIFO working mode
00013     i2c_write(LSM6DS0_ADDRESS, LSM6DS0_SUB_FIFO_CTRL, data_write, 1);
00014     data_write[0] = 0x50;                               // set FIFO-full and overrun interrupts
00015     i2c_write(LSM6DS0_ADDRESS, LSM6DS0_SUB_INT_CTRL, data_write, 1);
00016     data_write[0] = 0x02;                               // enable FIFO. To enable FIFO_threshold stop set value 3
00017     i2c_write(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG9, data_write, 1);
00018     
00019 }
00020 
00021 void LSM6DS0::resetFIFO(void)
00022 {
00023     char data_write[1];
00024     LSM6DS0_FIFO_mode_t work_mode;
00025     work_mode = LSM6DS0_FIFO_mode_BYPASS;
00026     data_write[0] = work_mode;                          // set FIFO working mode
00027     i2c_write(LSM6DS0_ADDRESS, LSM6DS0_SUB_FIFO_CTRL, data_write, 1);
00028     
00029 }
00030 
00031 void LSM6DS0::getFIFO(void)
00032 {
00033     /* This function reads the content of the entire FIFO */
00034     char data_read[192]; //3 axes * 2 read for each ax * 32 FIFO depth = 192
00035     
00036     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_OUT_X_L_XL, data_read, 192);
00037     
00038     for(int i=0; i<32; i++)
00039     {
00040         printf("x = %d, y = %d, z = %d\n\r", (int16_t)((data_read[1+i*6] << 8)| data_read[0+i*6]), (int16_t)((data_read[3+i*6] << 8)| data_read[2+i*6]), (int16_t)((data_read[5+i*6] << 8)| data_read[4+i*6]));
00041     }
00042 }
00043 
00044 void LSM6DS0::storeFIFO(int *offset, xl_output *data_FIFO)
00045 {
00046     int i;
00047     
00048     /* This function reads the content of the entire FIFO */
00049     char data_read[192]; //3 axes * 2 read for each ax * 32 FIFO depth = 192
00050     
00051     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_OUT_X_L_XL, data_read, 192);
00052     
00053     if(*offset == FIFO_length_by_five)
00054     {
00055         *offset = 0;
00056     }
00057     
00058     for(i=0; i<32; i++)
00059     {
00060         
00061         data_FIFO[i+*offset].x = (int16_t)((data_read[1+i*6] << 8)| data_read[0+i*6]);
00062         data_FIFO[i+*offset].y = (int16_t)((data_read[3+i*6] << 8)| data_read[2+i*6]);
00063         data_FIFO[i+*offset].z = (int16_t)((data_read[5+i*6] << 8)| data_read[4+i*6]);
00064         
00065     }
00066     *offset = *offset + i;
00067 }
00068 
00069 void LSM6DS0::computeValue(xl_output *data_FIFO)
00070  {
00071      int i;
00072      float sensValue = getSensitivity();
00073      
00074      for(i=0; i<FIFO_length_by_five; i++)
00075      {
00076      pc.printf("X : %f ", (float)(data_FIFO[i].x*sensValue));
00077      pc.printf("Y : %f ", (float)(data_FIFO[i].y*sensValue));
00078      pc.printf("Z : %f \n\r", (float)(data_FIFO[i].z*sensValue));
00079      }
00080  }
00081  
00082 float LSM6DS0::getSensitivity(void)
00083 {
00084     char data_read[1];
00085     float sensValue;
00086     
00087     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG6_XL, data_read, 1);
00088     
00089     switch(data_read[0] & 0x18) {
00090         case 0x00:
00091             sensValue = LSM6DS0_ACC_SENS_2G;
00092             break;
00093         case 0x08:
00094             sensValue = LSM6DS0_ACC_SENS_16G;
00095             break;
00096         case 0x10:
00097             sensValue = LSM6DS0_ACC_SENS_4G;
00098             break;
00099         case 0x18:
00100             sensValue = LSM6DS0_ACC_SENS_8G;
00101             break;
00102         default:
00103             sensValue = 1;
00104             pc.printf("\n\rSensitivity error. No value\n\r");
00105             break;
00106     }
00107     
00108     return sensValue;
00109 }
00110 
00111 void LSM6DS0::Init(LSM6DS0_ACC_ODR_t odr, LSM6DS0_ACC_HR_t high_res, LSM6DS0_FIFO_mode_t work_mode)
00112 {
00113     char data_write[1];
00114     data_write[0] = odr;            // Set frequency
00115     i2c_write(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG6_XL, data_write, 1);
00116     data_write[0] = high_res;       // Set High-Resolution
00117     i2c_write(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG7_XL, data_write, 1);
00118     
00119     setFIFO(work_mode);
00120 }
00121 
00122 void LSM6DS0::LSM6DS0_reset(void)
00123 {
00124     char data_write[1];
00125     data_write[0] = 0x01;            // Software reset
00126     i2c_write(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG8, data_write, 1);
00127 }
00128 
00129 void LSM6DS0::printValue(xl_output *data_FIFO)
00130 {
00131     int i;
00132     for(i=0; i<FIFO_length_by_five; i++)
00133     {
00134         pc.printf("\r\n");
00135         pc.printf("%d: x = %d, y = %d, z = %d\n\r", i, data_FIFO[i].x, data_FIFO[i].y, data_FIFO[i].z);
00136         
00137     }
00138 }
00139 
00140 void LSM6DS0::printReg(void)
00141 {
00142     char data_read[1];
00143     
00144     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG6_XL, data_read, 1);
00145     pc.printf("CTRL_REG6_XL = %#4X\r\n", data_read[0]);
00146     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG7_XL, data_read, 1);
00147     pc.printf("CTRL_REG7_XL = %#4X\r\n", data_read[0]);
00148     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_FIFO_CTRL, data_read, 1);
00149     pc.printf("SUB_FIFO_CTRL = %#4X\r\n", data_read[0]);
00150     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_INT_CTRL, data_read, 1);
00151     pc.printf("INT_CTRL = %#4X\r\n", data_read[0]);
00152     i2c_read(LSM6DS0_ADDRESS, LSM6DS0_SUB_CTRL_REG9, data_read, 1);
00153     pc.printf("CTRL_REG9 = %#4X\r\n", data_read[0]);
00154 }