Class library for a LIS3DSH MEMS digital output motion sensor (acceleromoter).
Dependents: stm32f407 mbed_blinky STM32F407
LIS3DSH.cpp@0:36febce4f85e, 2016-08-09 (annotated)
- Committer:
- grantphillips
- Date:
- Tue Aug 09 13:30:34 2016 +0000
- Revision:
- 0:36febce4f85e
v1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
grantphillips | 0:36febce4f85e | 1 | #include "LIS3DSH.h" |
grantphillips | 0:36febce4f85e | 2 | #include "mbed.h" |
grantphillips | 0:36febce4f85e | 3 | |
grantphillips | 0:36febce4f85e | 4 | #define LIS3DSH_INFO1 0x0D |
grantphillips | 0:36febce4f85e | 5 | #define LIS3DSH_INFO2 0x0E |
grantphillips | 0:36febce4f85e | 6 | #define LIS3DSH_WHO_AM_I 0x0F |
grantphillips | 0:36febce4f85e | 7 | #define LIS3DSH_OFF_X 0x10 |
grantphillips | 0:36febce4f85e | 8 | #define LIS3DSH_OFF_Y 0x11 |
grantphillips | 0:36febce4f85e | 9 | #define LIS3DSH_OFF_Z 0x12 |
grantphillips | 0:36febce4f85e | 10 | #define LIS3DSH_CTRL_REG4 0x20 |
grantphillips | 0:36febce4f85e | 11 | #define LIS3DSH_CTRL_REG1 0x21 |
grantphillips | 0:36febce4f85e | 12 | #define LIS3DSH_CTRL_REG2 0x22 |
grantphillips | 0:36febce4f85e | 13 | #define LIS3DSH_CTRL_REG3 0x23 |
grantphillips | 0:36febce4f85e | 14 | #define LIS3DSH_CTRL_REG5 0x24 |
grantphillips | 0:36febce4f85e | 15 | #define LIS3DSH_CTRL_REG6 0x25 |
grantphillips | 0:36febce4f85e | 16 | #define LIS3DSH_OUT_X_L 0x28 |
grantphillips | 0:36febce4f85e | 17 | #define LIS3DSH_OUT_X_H 0x29 |
grantphillips | 0:36febce4f85e | 18 | #define LIS3DSH_OUT_Y_L 0x2A |
grantphillips | 0:36febce4f85e | 19 | #define LIS3DSH_OUT_Y_H 0x2B |
grantphillips | 0:36febce4f85e | 20 | #define LIS3DSH_OUT_Z_L 0x2C |
grantphillips | 0:36febce4f85e | 21 | #define LIS3DSH_OUT_Z_H 0x2D |
grantphillips | 0:36febce4f85e | 22 | #define LIS3DSH_FIFO_CTRL_REG 0x2E |
grantphillips | 0:36febce4f85e | 23 | |
grantphillips | 0:36febce4f85e | 24 | #define LIS3DSH_READ 0x80 |
grantphillips | 0:36febce4f85e | 25 | #define LIS3DSH_WRITE 0x00 |
grantphillips | 0:36febce4f85e | 26 | |
grantphillips | 0:36febce4f85e | 27 | LIS3DSH::LIS3DSH(PinName mosi, PinName miso, PinName clk, PinName cs) |
grantphillips | 0:36febce4f85e | 28 | : _spi(mosi, miso, clk), _cs(cs) { |
grantphillips | 0:36febce4f85e | 29 | |
grantphillips | 0:36febce4f85e | 30 | // Make sure CS is high |
grantphillips | 0:36febce4f85e | 31 | _cs = 1; |
grantphillips | 0:36febce4f85e | 32 | |
grantphillips | 0:36febce4f85e | 33 | // Set up the spi interface |
grantphillips | 0:36febce4f85e | 34 | _spi.format(8, 3); |
grantphillips | 0:36febce4f85e | 35 | _spi.frequency(1000000); |
grantphillips | 0:36febce4f85e | 36 | |
grantphillips | 0:36febce4f85e | 37 | // Configure LIS3DSH |
grantphillips | 0:36febce4f85e | 38 | WriteReg(LIS3DSH_CTRL_REG4, 0x5F); // Normal power mode, all axes enabled, 50 Hz ODR |
grantphillips | 0:36febce4f85e | 39 | WriteReg(LIS3DSH_CTRL_REG5, 0x80); // 200 Hz antialias filter, +/- 2g FS range |
grantphillips | 0:36febce4f85e | 40 | WriteReg(LIS3DSH_FIFO_CTRL_REG, 0); // configure FIFO for bypass mode |
grantphillips | 0:36febce4f85e | 41 | WriteReg(LIS3DSH_CTRL_REG6, 0x10); // disable FIFO, enable register address auto-increment |
grantphillips | 0:36febce4f85e | 42 | |
grantphillips | 0:36febce4f85e | 43 | /* these two lines prevents lock-up of sampling according to: |
grantphillips | 0:36febce4f85e | 44 | https://my.st.com/public/STe2ecommunities/mems_sensors/Lists/Accelerometers/DispForm.aspx?ID=304&Source=/public/STe2ecommunities/mems_sensors/Tags.aspx?tags=sampling |
grantphillips | 0:36febce4f85e | 45 | |
grantphillips | 0:36febce4f85e | 46 | Not sure why it works |
grantphillips | 0:36febce4f85e | 47 | */ |
grantphillips | 0:36febce4f85e | 48 | WriteReg(LIS3DSH_CTRL_REG4, 0x00); |
grantphillips | 0:36febce4f85e | 49 | WriteReg(LIS3DSH_CTRL_REG4, 0x37); |
grantphillips | 0:36febce4f85e | 50 | } |
grantphillips | 0:36febce4f85e | 51 | |
grantphillips | 0:36febce4f85e | 52 | void LIS3DSH::WriteReg(uint8_t addr, uint8_t data) { |
grantphillips | 0:36febce4f85e | 53 | _cs = 0; |
grantphillips | 0:36febce4f85e | 54 | _spi.write(LIS3DSH_WRITE | addr); |
grantphillips | 0:36febce4f85e | 55 | _spi.write(data); |
grantphillips | 0:36febce4f85e | 56 | _cs = 1; |
grantphillips | 0:36febce4f85e | 57 | } |
grantphillips | 0:36febce4f85e | 58 | |
grantphillips | 0:36febce4f85e | 59 | uint8_t LIS3DSH::ReadReg(uint8_t addr) { |
grantphillips | 0:36febce4f85e | 60 | uint8_t data; |
grantphillips | 0:36febce4f85e | 61 | |
grantphillips | 0:36febce4f85e | 62 | _cs = 0; |
grantphillips | 0:36febce4f85e | 63 | _spi.write(LIS3DSH_READ | addr); |
grantphillips | 0:36febce4f85e | 64 | data = _spi.write(0x00); |
grantphillips | 0:36febce4f85e | 65 | _cs = 1; |
grantphillips | 0:36febce4f85e | 66 | |
grantphillips | 0:36febce4f85e | 67 | return(data); |
grantphillips | 0:36febce4f85e | 68 | } |
grantphillips | 0:36febce4f85e | 69 | |
grantphillips | 0:36febce4f85e | 70 | int LIS3DSH::Detect(void) { |
grantphillips | 0:36febce4f85e | 71 | if(ReadReg(LIS3DSH_WHO_AM_I) == 0x3F) |
grantphillips | 0:36febce4f85e | 72 | return(1); |
grantphillips | 0:36febce4f85e | 73 | else |
grantphillips | 0:36febce4f85e | 74 | return(0); |
grantphillips | 0:36febce4f85e | 75 | } |
grantphillips | 0:36febce4f85e | 76 | |
grantphillips | 0:36febce4f85e | 77 | void LIS3DSH::ReadData(int16_t *X, int16_t *Y, int16_t *Z) { |
grantphillips | 0:36febce4f85e | 78 | uint8_t xLSB=0, xMSB, yLSB, yMSB, zLSB, zMSB; // 8-bit values from accelerometer |
grantphillips | 0:36febce4f85e | 79 | |
grantphillips | 0:36febce4f85e | 80 | xMSB = ReadReg(LIS3DSH_OUT_X_H); // read X high byte register |
grantphillips | 0:36febce4f85e | 81 | xLSB = ReadReg(LIS3DSH_OUT_X_L); // read X low byte register |
grantphillips | 0:36febce4f85e | 82 | yMSB = ReadReg(LIS3DSH_OUT_Y_H); |
grantphillips | 0:36febce4f85e | 83 | yLSB = ReadReg(LIS3DSH_OUT_Y_L); |
grantphillips | 0:36febce4f85e | 84 | zMSB = ReadReg(LIS3DSH_OUT_Z_H); |
grantphillips | 0:36febce4f85e | 85 | zLSB = ReadReg(LIS3DSH_OUT_Z_L); |
grantphillips | 0:36febce4f85e | 86 | |
grantphillips | 0:36febce4f85e | 87 | //pack MSB and LSB bytes for X, Y, and Z |
grantphillips | 0:36febce4f85e | 88 | *X = (xMSB << 8) | (xLSB); |
grantphillips | 0:36febce4f85e | 89 | *Y = (yMSB << 8) | (yLSB); |
grantphillips | 0:36febce4f85e | 90 | *Z = (zMSB << 8) | (zLSB); |
grantphillips | 0:36febce4f85e | 91 | } |
grantphillips | 0:36febce4f85e | 92 | |
grantphillips | 0:36febce4f85e | 93 | void LIS3DSH::ReadAngles(float *Roll, float *Pitch) { |
grantphillips | 0:36febce4f85e | 94 | int16_t Xg, Yg, Zg; // 16-bit values from accelerometer |
grantphillips | 0:36febce4f85e | 95 | |
grantphillips | 0:36febce4f85e | 96 | //Read X, Y, Z raw values from acceleromoter |
grantphillips | 0:36febce4f85e | 97 | ReadData(&Xg, &Yg, &Zg); |
grantphillips | 0:36febce4f85e | 98 | |
grantphillips | 0:36febce4f85e | 99 | //Convert X, Y, and Z accelerometer values to meters per second squared |
grantphillips | 0:36febce4f85e | 100 | Xg /= -141; |
grantphillips | 0:36febce4f85e | 101 | Zg /= -141; |
grantphillips | 0:36febce4f85e | 102 | Yg /= -141; |
grantphillips | 0:36febce4f85e | 103 | |
grantphillips | 0:36febce4f85e | 104 | *Roll = gToDegrees(Zg, Xg); // get degrees between Z and X planes |
grantphillips | 0:36febce4f85e | 105 | *Pitch = gToDegrees(Zg, Yg); // get degrees between Z and Y planes |
grantphillips | 0:36febce4f85e | 106 | } |
grantphillips | 0:36febce4f85e | 107 | |
grantphillips | 0:36febce4f85e | 108 | float LIS3DSH::gToDegrees(float V, float H) |
grantphillips | 0:36febce4f85e | 109 | { |
grantphillips | 0:36febce4f85e | 110 | float retval; |
grantphillips | 0:36febce4f85e | 111 | uint16_t orientation=0; |
grantphillips | 0:36febce4f85e | 112 | |
grantphillips | 0:36febce4f85e | 113 | if (H == 0) H = 0.001; // preventing division by zero |
grantphillips | 0:36febce4f85e | 114 | if (V == 0) V = 0.001; // preventing division by zero |
grantphillips | 0:36febce4f85e | 115 | |
grantphillips | 0:36febce4f85e | 116 | if ((H > 0) && (V > 0)) orientation = 0; |
grantphillips | 0:36febce4f85e | 117 | if ((H < 0) && (V > 0)) orientation = 90; |
grantphillips | 0:36febce4f85e | 118 | if ((H < 0) && (V < 0)) orientation = 180; |
grantphillips | 0:36febce4f85e | 119 | if ((H > 0) && (V < 0)) orientation = 270; |
grantphillips | 0:36febce4f85e | 120 | |
grantphillips | 0:36febce4f85e | 121 | retval = (atan((double)V/(double)H)/3.14159)*180.0; |
grantphillips | 0:36febce4f85e | 122 | if ((double)retval < 0) |
grantphillips | 0:36febce4f85e | 123 | retval = (double)retval + 90.0; |
grantphillips | 0:36febce4f85e | 124 | retval = fabs(retval) + orientation; |
grantphillips | 0:36febce4f85e | 125 | return retval; |
grantphillips | 0:36febce4f85e | 126 | } |