Class library for a LIS3DSH MEMS digital output motion sensor (acceleromoter).
Dependents: stm32f407 mbed_blinky STM32F407
Diff: LIS3DSH.cpp
- Revision:
- 0:36febce4f85e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LIS3DSH.cpp Tue Aug 09 13:30:34 2016 +0000 @@ -0,0 +1,126 @@ +#include "LIS3DSH.h" +#include "mbed.h" + +#define LIS3DSH_INFO1 0x0D +#define LIS3DSH_INFO2 0x0E +#define LIS3DSH_WHO_AM_I 0x0F +#define LIS3DSH_OFF_X 0x10 +#define LIS3DSH_OFF_Y 0x11 +#define LIS3DSH_OFF_Z 0x12 +#define LIS3DSH_CTRL_REG4 0x20 +#define LIS3DSH_CTRL_REG1 0x21 +#define LIS3DSH_CTRL_REG2 0x22 +#define LIS3DSH_CTRL_REG3 0x23 +#define LIS3DSH_CTRL_REG5 0x24 +#define LIS3DSH_CTRL_REG6 0x25 +#define LIS3DSH_OUT_X_L 0x28 +#define LIS3DSH_OUT_X_H 0x29 +#define LIS3DSH_OUT_Y_L 0x2A +#define LIS3DSH_OUT_Y_H 0x2B +#define LIS3DSH_OUT_Z_L 0x2C +#define LIS3DSH_OUT_Z_H 0x2D +#define LIS3DSH_FIFO_CTRL_REG 0x2E + +#define LIS3DSH_READ 0x80 +#define LIS3DSH_WRITE 0x00 + +LIS3DSH::LIS3DSH(PinName mosi, PinName miso, PinName clk, PinName cs) + : _spi(mosi, miso, clk), _cs(cs) { + + // Make sure CS is high + _cs = 1; + + // Set up the spi interface + _spi.format(8, 3); + _spi.frequency(1000000); + + // Configure LIS3DSH + WriteReg(LIS3DSH_CTRL_REG4, 0x5F); // Normal power mode, all axes enabled, 50 Hz ODR + WriteReg(LIS3DSH_CTRL_REG5, 0x80); // 200 Hz antialias filter, +/- 2g FS range + WriteReg(LIS3DSH_FIFO_CTRL_REG, 0); // configure FIFO for bypass mode + WriteReg(LIS3DSH_CTRL_REG6, 0x10); // disable FIFO, enable register address auto-increment + + /* these two lines prevents lock-up of sampling according to: + https://my.st.com/public/STe2ecommunities/mems_sensors/Lists/Accelerometers/DispForm.aspx?ID=304&Source=/public/STe2ecommunities/mems_sensors/Tags.aspx?tags=sampling + + Not sure why it works + */ + WriteReg(LIS3DSH_CTRL_REG4, 0x00); + WriteReg(LIS3DSH_CTRL_REG4, 0x37); +} + +void LIS3DSH::WriteReg(uint8_t addr, uint8_t data) { + _cs = 0; + _spi.write(LIS3DSH_WRITE | addr); + _spi.write(data); + _cs = 1; +} + +uint8_t LIS3DSH::ReadReg(uint8_t addr) { + uint8_t data; + + _cs = 0; + _spi.write(LIS3DSH_READ | addr); + data = _spi.write(0x00); + _cs = 1; + + return(data); +} + +int LIS3DSH::Detect(void) { + if(ReadReg(LIS3DSH_WHO_AM_I) == 0x3F) + return(1); + else + return(0); +} + +void LIS3DSH::ReadData(int16_t *X, int16_t *Y, int16_t *Z) { + uint8_t xLSB=0, xMSB, yLSB, yMSB, zLSB, zMSB; // 8-bit values from accelerometer + + xMSB = ReadReg(LIS3DSH_OUT_X_H); // read X high byte register + xLSB = ReadReg(LIS3DSH_OUT_X_L); // read X low byte register + yMSB = ReadReg(LIS3DSH_OUT_Y_H); + yLSB = ReadReg(LIS3DSH_OUT_Y_L); + zMSB = ReadReg(LIS3DSH_OUT_Z_H); + zLSB = ReadReg(LIS3DSH_OUT_Z_L); + + //pack MSB and LSB bytes for X, Y, and Z + *X = (xMSB << 8) | (xLSB); + *Y = (yMSB << 8) | (yLSB); + *Z = (zMSB << 8) | (zLSB); +} + +void LIS3DSH::ReadAngles(float *Roll, float *Pitch) { + int16_t Xg, Yg, Zg; // 16-bit values from accelerometer + + //Read X, Y, Z raw values from acceleromoter + ReadData(&Xg, &Yg, &Zg); + + //Convert X, Y, and Z accelerometer values to meters per second squared + Xg /= -141; + Zg /= -141; + Yg /= -141; + + *Roll = gToDegrees(Zg, Xg); // get degrees between Z and X planes + *Pitch = gToDegrees(Zg, Yg); // get degrees between Z and Y planes +} + +float LIS3DSH::gToDegrees(float V, float H) +{ + float retval; + uint16_t orientation=0; + + if (H == 0) H = 0.001; // preventing division by zero + if (V == 0) V = 0.001; // preventing division by zero + + if ((H > 0) && (V > 0)) orientation = 0; + if ((H < 0) && (V > 0)) orientation = 90; + if ((H < 0) && (V < 0)) orientation = 180; + if ((H > 0) && (V < 0)) orientation = 270; + + retval = (atan((double)V/(double)H)/3.14159)*180.0; + if ((double)retval < 0) + retval = (double)retval + 90.0; + retval = fabs(retval) + orientation; + return retval; +}