LIS3DSH.cpp

00001 #include "LIS3DSH.h"
00002 #include "mbed.h"
00003  
00004 #define LIS3DSH_INFO1                       0x0D
00005 #define LIS3DSH_INFO2                       0x0E
00006 #define LIS3DSH_WHO_AM_I                    0x0F
00007 #define LIS3DSH_OFF_X                       0x10
00008 #define LIS3DSH_OFF_Y                       0x11
00009 #define LIS3DSH_OFF_Z                       0x12
00010 #define LIS3DSH_CTRL_REG4                   0x20
00011 #define LIS3DSH_CTRL_REG1                   0x21
00012 #define LIS3DSH_CTRL_REG2                   0x22
00013 #define LIS3DSH_CTRL_REG3                   0x23
00014 #define LIS3DSH_CTRL_REG5                   0x24
00015 #define LIS3DSH_CTRL_REG6                   0x25
00016 #define LIS3DSH_OUT_X_L                     0x28
00017 #define LIS3DSH_OUT_X_H                     0x29
00018 #define LIS3DSH_OUT_Y_L                     0x2A
00019 #define LIS3DSH_OUT_Y_H                     0x2B
00020 #define LIS3DSH_OUT_Z_L                     0x2C
00021 #define LIS3DSH_OUT_Z_H                     0x2D
00022 #define LIS3DSH_FIFO_CTRL_REG               0x2E
00023 
00024 #define LIS3DSH_READ                        0x80
00025 #define LIS3DSH_WRITE                       0x00
00026 
00027 LIS3DSH::LIS3DSH(PinName mosi, PinName miso, PinName clk, PinName cs)
00028     : _spi(mosi, miso, clk), _cs(cs) {
00029     
00030     // Make sure CS is high
00031     _cs = 1;
00032  
00033     // Set up the spi interface
00034     _spi.format(8, 3);
00035     _spi.frequency(1000000);
00036     
00037     // Configure LIS3DSH
00038     WriteReg(LIS3DSH_CTRL_REG4, 0x5F);             // Normal power mode, all axes enabled, 50 Hz ODR
00039     WriteReg(LIS3DSH_CTRL_REG5, 0x80);             // 200 Hz antialias filter, +/- 2g FS range   
00040     WriteReg(LIS3DSH_FIFO_CTRL_REG, 0);            // configure FIFO for bypass mode   
00041     WriteReg(LIS3DSH_CTRL_REG6, 0x10);             // disable FIFO, enable register address auto-increment
00042 
00043     /* these two lines prevents lock-up of sampling according to:
00044     https://my.st.com/public/STe2ecommunities/mems_sensors/Lists/Accelerometers/DispForm.aspx?ID=304&Source=/public/STe2ecommunities/mems_sensors/Tags.aspx?tags=sampling
00045     
00046     Not sure why it works
00047     */
00048     WriteReg(LIS3DSH_CTRL_REG4, 0x00);
00049     WriteReg(LIS3DSH_CTRL_REG4, 0x37);
00050 }
00051 
00052 void LIS3DSH::WriteReg(uint8_t addr, uint8_t data) {
00053     _cs = 0;
00054     _spi.write(LIS3DSH_WRITE | addr);
00055     _spi.write(data);
00056     _cs = 1;
00057 }
00058 
00059 uint8_t LIS3DSH::ReadReg(uint8_t addr) {
00060     uint8_t data;
00061     
00062     _cs = 0;           
00063     _spi.write(LIS3DSH_READ | addr);
00064     data = _spi.write(0x00);             
00065     _cs = 1;
00066     
00067     return(data);
00068 }
00069 
00070 int LIS3DSH::Detect(void) {
00071     if(ReadReg(LIS3DSH_WHO_AM_I) == 0x3F)
00072         return(1);
00073     else
00074         return(0);
00075 }
00076 
00077 void LIS3DSH::ReadData(int16_t *X, int16_t *Y, int16_t *Z) {
00078     uint8_t xLSB=0, xMSB, yLSB, yMSB, zLSB, zMSB;       // 8-bit values from accelerometer
00079     
00080     xMSB = ReadReg(LIS3DSH_OUT_X_H);                    // read X high byte register
00081     xLSB = ReadReg(LIS3DSH_OUT_X_L);                     // read X low byte register
00082     yMSB = ReadReg(LIS3DSH_OUT_Y_H);
00083     yLSB = ReadReg(LIS3DSH_OUT_Y_L);
00084     zMSB = ReadReg(LIS3DSH_OUT_Z_H);
00085     zLSB = ReadReg(LIS3DSH_OUT_Z_L);
00086         
00087     //pack MSB and LSB bytes for X, Y, and Z
00088     *X = (xMSB << 8) | (xLSB);   
00089     *Y = (yMSB << 8) | (yLSB);
00090     *Z = (zMSB << 8) | (zLSB);
00091 }
00092 
00093 void LIS3DSH::ReadAngles(float *Roll, float *Pitch) {   
00094     int16_t Xg, Yg, Zg;                          // 16-bit values from accelerometer
00095 
00096     //Read X, Y, Z raw values from acceleromoter     
00097     ReadData(&Xg, &Yg, &Zg);
00098     
00099     //Convert X, Y, and Z accelerometer values to meters per second squared
00100     Xg /= -141;                          
00101     Zg /= -141;                          
00102     Yg /= -141;
00103         
00104     *Roll = gToDegrees(Zg, Xg);                  // get degrees between Z and X planes
00105     *Pitch = gToDegrees(Zg, Yg);                 // get degrees between Z and Y planes
00106 }
00107 
00108 float LIS3DSH::gToDegrees(float V, float H)      
00109 {
00110   float retval;
00111   uint16_t orientation=0;
00112  
00113   if (H == 0) H = 0.001;                         // preventing division by zero
00114   if (V == 0) V = 0.001;                         // preventing division by zero
00115  
00116   if ((H > 0) && (V > 0)) orientation = 0;
00117   if ((H < 0) && (V > 0)) orientation = 90; 
00118   if ((H < 0) && (V < 0)) orientation = 180;
00119   if ((H > 0) && (V < 0)) orientation = 270;
00120  
00121   retval = (atan((double)V/(double)H)/3.14159)*180.0;
00122   if ((double)retval < 0) 
00123     retval = (double)retval + 90.0;
00124   retval = fabs(retval) + orientation;
00125   return retval;
00126 }