Allows for reading accelerometer, gyroscope, and magnetometer data from an LSM9DS0 IMU device
Dependents: uVGA_4180 uLCD_4180_mini ECE4781_Project
LSM9DS0.cpp@4:bf8f4e7c9905, 2014-12-03 (annotated)
- Committer:
- randrews33
- Date:
- Wed Dec 03 23:08:09 2014 +0000
- Revision:
- 4:bf8f4e7c9905
- Parent:
- 0:1b975a6ae539
Issues with the previous versions caused very strange values to be read back. This was due to the wrong addresses being read. Fixed here
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
randrews33 | 0:1b975a6ae539 | 1 | #include "LSM9DS0.h" |
randrews33 | 0:1b975a6ae539 | 2 | |
randrews33 | 0:1b975a6ae539 | 3 | LSM9DS0::LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr) |
randrews33 | 0:1b975a6ae539 | 4 | { |
randrews33 | 0:1b975a6ae539 | 5 | // xmAddress and gAddress will store the 7-bit I2C address, if using I2C. |
randrews33 | 0:1b975a6ae539 | 6 | xmAddress = xmAddr; |
randrews33 | 0:1b975a6ae539 | 7 | gAddress = gAddr; |
randrews33 | 0:1b975a6ae539 | 8 | |
randrews33 | 0:1b975a6ae539 | 9 | i2c_ = new I2Cdev(sda, scl); |
randrews33 | 0:1b975a6ae539 | 10 | } |
randrews33 | 0:1b975a6ae539 | 11 | |
randrews33 | 0:1b975a6ae539 | 12 | uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, |
randrews33 | 0:1b975a6ae539 | 13 | gyro_odr gODR, accel_odr aODR, mag_odr mODR) |
randrews33 | 0:1b975a6ae539 | 14 | { |
randrews33 | 0:1b975a6ae539 | 15 | // Store the given scales in class variables. These scale variables |
randrews33 | 0:1b975a6ae539 | 16 | // are used throughout to calculate the actual g's, DPS,and Gs's. |
randrews33 | 0:1b975a6ae539 | 17 | gScale = gScl; |
randrews33 | 0:1b975a6ae539 | 18 | aScale = aScl; |
randrews33 | 0:1b975a6ae539 | 19 | mScale = mScl; |
randrews33 | 0:1b975a6ae539 | 20 | |
randrews33 | 0:1b975a6ae539 | 21 | // Once we have the scale values, we can calculate the resolution |
randrews33 | 0:1b975a6ae539 | 22 | // of each sensor. That's what these functions are for. One for each sensor |
randrews33 | 0:1b975a6ae539 | 23 | calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable |
randrews33 | 0:1b975a6ae539 | 24 | calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable |
randrews33 | 0:1b975a6ae539 | 25 | calcaRes(); // Calculate g / ADC tick, stored in aRes variable |
randrews33 | 0:1b975a6ae539 | 26 | |
randrews33 | 0:1b975a6ae539 | 27 | |
randrews33 | 0:1b975a6ae539 | 28 | // To verify communication, we can read from the WHO_AM_I register of |
randrews33 | 0:1b975a6ae539 | 29 | // each device. Store those in a variable so we can return them. |
randrews33 | 0:1b975a6ae539 | 30 | uint8_t gTest = gReadByte(WHO_AM_I_G); // Read the gyro WHO_AM_I |
randrews33 | 0:1b975a6ae539 | 31 | uint8_t xmTest = xmReadByte(WHO_AM_I_XM); // Read the accel/mag WHO_AM_I |
randrews33 | 0:1b975a6ae539 | 32 | |
randrews33 | 0:1b975a6ae539 | 33 | // Gyro initialization stuff: |
randrews33 | 0:1b975a6ae539 | 34 | initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc. |
randrews33 | 0:1b975a6ae539 | 35 | setGyroODR(gODR); // Set the gyro output data rate and bandwidth. |
randrews33 | 0:1b975a6ae539 | 36 | setGyroScale(gScale); // Set the gyro range |
randrews33 | 0:1b975a6ae539 | 37 | |
randrews33 | 0:1b975a6ae539 | 38 | // Accelerometer initialization stuff: |
randrews33 | 0:1b975a6ae539 | 39 | initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc. |
randrews33 | 4:bf8f4e7c9905 | 40 | setAccelODR(aODR); // Set the accel data rate. |
randrews33 | 4:bf8f4e7c9905 | 41 | setAccelScale(aScale); // Set the accel range. |
randrews33 | 0:1b975a6ae539 | 42 | |
randrews33 | 0:1b975a6ae539 | 43 | // Magnetometer initialization stuff: |
randrews33 | 0:1b975a6ae539 | 44 | initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc. |
randrews33 | 0:1b975a6ae539 | 45 | setMagODR(mODR); // Set the magnetometer output data rate. |
randrews33 | 0:1b975a6ae539 | 46 | setMagScale(mScale); // Set the magnetometer's range. |
randrews33 | 0:1b975a6ae539 | 47 | |
randrews33 | 0:1b975a6ae539 | 48 | // Once everything is initialized, return the WHO_AM_I registers we read: |
randrews33 | 0:1b975a6ae539 | 49 | return (xmTest << 8) | gTest; |
randrews33 | 0:1b975a6ae539 | 50 | } |
randrews33 | 0:1b975a6ae539 | 51 | |
randrews33 | 0:1b975a6ae539 | 52 | void LSM9DS0::initGyro() |
randrews33 | 0:1b975a6ae539 | 53 | { |
randrews33 | 4:bf8f4e7c9905 | 54 | |
randrews33 | 0:1b975a6ae539 | 55 | gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes |
randrews33 | 0:1b975a6ae539 | 56 | gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency |
randrews33 | 4:bf8f4e7c9905 | 57 | gWriteByte(CTRL_REG3_G, 0x88); //Interrupt enabled on both INT_G and I2_DRDY |
randrews33 | 4:bf8f4e7c9905 | 58 | gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps |
randrews33 | 4:bf8f4e7c9905 | 59 | gWriteByte(CTRL_REG5_G, 0x00); //Init default values |
randrews33 | 0:1b975a6ae539 | 60 | |
randrews33 | 0:1b975a6ae539 | 61 | } |
randrews33 | 0:1b975a6ae539 | 62 | |
randrews33 | 0:1b975a6ae539 | 63 | void LSM9DS0::initAccel() |
randrews33 | 0:1b975a6ae539 | 64 | { |
randrews33 | 4:bf8f4e7c9905 | 65 | xmWriteByte(CTRL_REG0_XM, 0x00); |
randrews33 | 4:bf8f4e7c9905 | 66 | xmWriteByte(CTRL_REG1_XM, 0x57); // 50Hz data rate, x/y/z all enabled |
randrews33 | 0:1b975a6ae539 | 67 | xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g |
randrews33 | 4:bf8f4e7c9905 | 68 | xmWriteByte(CTRL_REG3_XM, 0x04); // Accelerometer data ready on INT1_XM (0x04) |
randrews33 | 4:bf8f4e7c9905 | 69 | |
randrews33 | 0:1b975a6ae539 | 70 | } |
randrews33 | 0:1b975a6ae539 | 71 | |
randrews33 | 0:1b975a6ae539 | 72 | void LSM9DS0::initMag() |
randrews33 | 0:1b975a6ae539 | 73 | { |
randrews33 | 4:bf8f4e7c9905 | 74 | xmWriteByte(CTRL_REG5_XM, 0x94); // Mag data rate - 100 Hz, enable temperature sensor |
randrews33 | 0:1b975a6ae539 | 75 | xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS |
randrews33 | 0:1b975a6ae539 | 76 | xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode |
randrews33 | 0:1b975a6ae539 | 77 | xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08) |
randrews33 | 0:1b975a6ae539 | 78 | xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull |
randrews33 | 0:1b975a6ae539 | 79 | } |
randrews33 | 0:1b975a6ae539 | 80 | |
randrews33 | 4:bf8f4e7c9905 | 81 | void LSM9DS0::calLSM9DS0(float * gbias, float * abias) |
randrews33 | 4:bf8f4e7c9905 | 82 | { |
randrews33 | 4:bf8f4e7c9905 | 83 | uint8_t data[6] = {0, 0, 0, 0, 0, 0}; |
randrews33 | 4:bf8f4e7c9905 | 84 | int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; |
randrews33 | 4:bf8f4e7c9905 | 85 | int samples, ii; |
randrews33 | 4:bf8f4e7c9905 | 86 | |
randrews33 | 4:bf8f4e7c9905 | 87 | // First get gyro bias |
randrews33 | 4:bf8f4e7c9905 | 88 | uint8_t c = gReadByte(CTRL_REG5_G); |
randrews33 | 4:bf8f4e7c9905 | 89 | gWriteByte(CTRL_REG5_G, c | 0x40); // Enable gyro FIFO |
randrews33 | 4:bf8f4e7c9905 | 90 | wait_ms(20); // Wait for change to take effect |
randrews33 | 4:bf8f4e7c9905 | 91 | gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples |
randrews33 | 4:bf8f4e7c9905 | 92 | wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples |
randrews33 | 4:bf8f4e7c9905 | 93 | |
randrews33 | 4:bf8f4e7c9905 | 94 | samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples |
randrews33 | 4:bf8f4e7c9905 | 95 | |
randrews33 | 4:bf8f4e7c9905 | 96 | for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO |
randrews33 | 4:bf8f4e7c9905 | 97 | |
randrews33 | 4:bf8f4e7c9905 | 98 | data[0] = gReadByte(OUT_X_L_G); |
randrews33 | 4:bf8f4e7c9905 | 99 | data[1] = gReadByte(OUT_X_H_G); |
randrews33 | 4:bf8f4e7c9905 | 100 | data[2] = gReadByte(OUT_Y_L_G); |
randrews33 | 4:bf8f4e7c9905 | 101 | data[3] = gReadByte(OUT_Y_H_G); |
randrews33 | 4:bf8f4e7c9905 | 102 | data[4] = gReadByte(OUT_Z_L_G); |
randrews33 | 4:bf8f4e7c9905 | 103 | data[5] = gReadByte(OUT_Z_H_G); |
randrews33 | 4:bf8f4e7c9905 | 104 | |
randrews33 | 4:bf8f4e7c9905 | 105 | gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]); |
randrews33 | 4:bf8f4e7c9905 | 106 | gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]); |
randrews33 | 4:bf8f4e7c9905 | 107 | gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]); |
randrews33 | 4:bf8f4e7c9905 | 108 | } |
randrews33 | 4:bf8f4e7c9905 | 109 | |
randrews33 | 4:bf8f4e7c9905 | 110 | gyro_bias[0] /= samples; // average the data |
randrews33 | 4:bf8f4e7c9905 | 111 | gyro_bias[1] /= samples; |
randrews33 | 4:bf8f4e7c9905 | 112 | gyro_bias[2] /= samples; |
randrews33 | 4:bf8f4e7c9905 | 113 | |
randrews33 | 4:bf8f4e7c9905 | 114 | gbias[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s |
randrews33 | 4:bf8f4e7c9905 | 115 | gbias[1] = (float)gyro_bias[1]*gRes; |
randrews33 | 4:bf8f4e7c9905 | 116 | gbias[2] = (float)gyro_bias[2]*gRes; |
randrews33 | 4:bf8f4e7c9905 | 117 | |
randrews33 | 4:bf8f4e7c9905 | 118 | c = gReadByte(CTRL_REG5_G); |
randrews33 | 4:bf8f4e7c9905 | 119 | gWriteByte(CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO |
randrews33 | 4:bf8f4e7c9905 | 120 | wait_ms(20); |
randrews33 | 4:bf8f4e7c9905 | 121 | gWriteByte(FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode |
randrews33 | 4:bf8f4e7c9905 | 122 | |
randrews33 | 4:bf8f4e7c9905 | 123 | // Now get the accelerometer biases |
randrews33 | 4:bf8f4e7c9905 | 124 | c = xmReadByte(CTRL_REG0_XM); |
randrews33 | 4:bf8f4e7c9905 | 125 | xmWriteByte(CTRL_REG0_XM, c | 0x40); // Enable accelerometer FIFO |
randrews33 | 4:bf8f4e7c9905 | 126 | wait_ms(20); // Wait for change to take effect |
randrews33 | 4:bf8f4e7c9905 | 127 | xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples |
randrews33 | 4:bf8f4e7c9905 | 128 | wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples |
randrews33 | 4:bf8f4e7c9905 | 129 | |
randrews33 | 4:bf8f4e7c9905 | 130 | samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples |
randrews33 | 4:bf8f4e7c9905 | 131 | |
randrews33 | 4:bf8f4e7c9905 | 132 | for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO |
randrews33 | 4:bf8f4e7c9905 | 133 | |
randrews33 | 4:bf8f4e7c9905 | 134 | data[0] = xmReadByte(OUT_X_L_A); |
randrews33 | 4:bf8f4e7c9905 | 135 | data[1] = xmReadByte(OUT_X_H_A); |
randrews33 | 4:bf8f4e7c9905 | 136 | data[2] = xmReadByte(OUT_Y_L_A); |
randrews33 | 4:bf8f4e7c9905 | 137 | data[3] = xmReadByte(OUT_Y_H_A); |
randrews33 | 4:bf8f4e7c9905 | 138 | data[4] = xmReadByte(OUT_Z_L_A); |
randrews33 | 4:bf8f4e7c9905 | 139 | data[5] = xmReadByte(OUT_Z_H_A); |
randrews33 | 4:bf8f4e7c9905 | 140 | accel_bias[0] += (((int16_t)data[1] << 8) | data[0]); |
randrews33 | 4:bf8f4e7c9905 | 141 | accel_bias[1] += (((int16_t)data[3] << 8) | data[2]); |
randrews33 | 4:bf8f4e7c9905 | 142 | accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1./aRes); // Assumes sensor facing up! |
randrews33 | 4:bf8f4e7c9905 | 143 | } |
randrews33 | 4:bf8f4e7c9905 | 144 | |
randrews33 | 4:bf8f4e7c9905 | 145 | accel_bias[0] /= samples; // average the data |
randrews33 | 4:bf8f4e7c9905 | 146 | accel_bias[1] /= samples; |
randrews33 | 4:bf8f4e7c9905 | 147 | accel_bias[2] /= samples; |
randrews33 | 4:bf8f4e7c9905 | 148 | |
randrews33 | 4:bf8f4e7c9905 | 149 | abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs |
randrews33 | 4:bf8f4e7c9905 | 150 | abias[1] = (float)accel_bias[1]*aRes; |
randrews33 | 4:bf8f4e7c9905 | 151 | abias[2] = (float)accel_bias[2]*aRes; |
randrews33 | 4:bf8f4e7c9905 | 152 | |
randrews33 | 4:bf8f4e7c9905 | 153 | c = xmReadByte(CTRL_REG0_XM); |
randrews33 | 4:bf8f4e7c9905 | 154 | xmWriteByte(CTRL_REG0_XM, c & ~0x40); // Disable accelerometer FIFO |
randrews33 | 4:bf8f4e7c9905 | 155 | wait_ms(20); |
randrews33 | 4:bf8f4e7c9905 | 156 | xmWriteByte(FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode |
randrews33 | 4:bf8f4e7c9905 | 157 | |
randrews33 | 4:bf8f4e7c9905 | 158 | } |
randrews33 | 0:1b975a6ae539 | 159 | void LSM9DS0::readAccel() |
randrews33 | 0:1b975a6ae539 | 160 | { |
randrews33 | 0:1b975a6ae539 | 161 | uint16_t Temp = 0; |
randrews33 | 0:1b975a6ae539 | 162 | |
randrews33 | 0:1b975a6ae539 | 163 | //Get x |
randrews33 | 0:1b975a6ae539 | 164 | Temp = xmReadByte(OUT_X_H_A); |
randrews33 | 0:1b975a6ae539 | 165 | Temp = Temp<<8; |
randrews33 | 0:1b975a6ae539 | 166 | Temp |= xmReadByte(OUT_X_L_A); |
randrews33 | 0:1b975a6ae539 | 167 | ax = Temp; |
randrews33 | 0:1b975a6ae539 | 168 | |
randrews33 | 0:1b975a6ae539 | 169 | |
randrews33 | 0:1b975a6ae539 | 170 | //Get y |
randrews33 | 0:1b975a6ae539 | 171 | Temp=0; |
randrews33 | 0:1b975a6ae539 | 172 | Temp = xmReadByte(OUT_Y_H_A); |
randrews33 | 0:1b975a6ae539 | 173 | Temp = Temp<<8; |
randrews33 | 0:1b975a6ae539 | 174 | Temp |= xmReadByte(OUT_Y_L_A); |
randrews33 | 0:1b975a6ae539 | 175 | ay = Temp; |
randrews33 | 0:1b975a6ae539 | 176 | |
randrews33 | 0:1b975a6ae539 | 177 | //Get z |
randrews33 | 0:1b975a6ae539 | 178 | Temp=0; |
randrews33 | 0:1b975a6ae539 | 179 | Temp = xmReadByte(OUT_Z_H_A); |
randrews33 | 0:1b975a6ae539 | 180 | Temp = Temp<<8; |
randrews33 | 0:1b975a6ae539 | 181 | Temp |= xmReadByte(OUT_Z_L_A); |
randrews33 | 0:1b975a6ae539 | 182 | az = Temp; |
randrews33 | 0:1b975a6ae539 | 183 | |
randrews33 | 0:1b975a6ae539 | 184 | } |
randrews33 | 0:1b975a6ae539 | 185 | |
randrews33 | 0:1b975a6ae539 | 186 | void LSM9DS0::readMag() |
randrews33 | 0:1b975a6ae539 | 187 | { |
randrews33 | 4:bf8f4e7c9905 | 188 | uint16_t Temp = 0; |
randrews33 | 0:1b975a6ae539 | 189 | |
randrews33 | 0:1b975a6ae539 | 190 | //Get x |
randrews33 | 0:1b975a6ae539 | 191 | Temp = xmReadByte(OUT_X_H_M); |
randrews33 | 0:1b975a6ae539 | 192 | Temp = Temp<<8; |
randrews33 | 0:1b975a6ae539 | 193 | Temp |= xmReadByte(OUT_X_L_M); |
randrews33 | 0:1b975a6ae539 | 194 | mx = Temp; |
randrews33 | 0:1b975a6ae539 | 195 | |
randrews33 | 0:1b975a6ae539 | 196 | |
randrews33 | 0:1b975a6ae539 | 197 | //Get y |
randrews33 | 0:1b975a6ae539 | 198 | Temp=0; |
randrews33 | 0:1b975a6ae539 | 199 | Temp = xmReadByte(OUT_Y_H_M); |
randrews33 | 0:1b975a6ae539 | 200 | Temp = Temp<<8; |
randrews33 | 0:1b975a6ae539 | 201 | Temp |= xmReadByte(OUT_Y_L_M); |
randrews33 | 0:1b975a6ae539 | 202 | my = Temp; |
randrews33 | 0:1b975a6ae539 | 203 | |
randrews33 | 0:1b975a6ae539 | 204 | //Get z |
randrews33 | 0:1b975a6ae539 | 205 | Temp=0; |
randrews33 | 0:1b975a6ae539 | 206 | Temp = xmReadByte(OUT_Z_H_M); |
randrews33 | 0:1b975a6ae539 | 207 | Temp = Temp<<8; |
randrews33 | 0:1b975a6ae539 | 208 | Temp |= xmReadByte(OUT_Z_L_M); |
randrews33 | 0:1b975a6ae539 | 209 | mz = Temp; |
randrews33 | 0:1b975a6ae539 | 210 | } |
randrews33 | 0:1b975a6ae539 | 211 | |
randrews33 | 4:bf8f4e7c9905 | 212 | void LSM9DS0::readTemp() |
randrews33 | 0:1b975a6ae539 | 213 | { |
randrews33 | 4:bf8f4e7c9905 | 214 | uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp |
randrews33 | 4:bf8f4e7c9905 | 215 | |
randrews33 | 4:bf8f4e7c9905 | 216 | temp[0] = xmReadByte(OUT_TEMP_L_XM); |
randrews33 | 4:bf8f4e7c9905 | 217 | temp[1] = xmReadByte(OUT_TEMP_H_XM); |
randrews33 | 0:1b975a6ae539 | 218 | |
randrews33 | 4:bf8f4e7c9905 | 219 | temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer |
randrews33 | 4:bf8f4e7c9905 | 220 | } |
randrews33 | 4:bf8f4e7c9905 | 221 | |
randrews33 | 4:bf8f4e7c9905 | 222 | |
randrews33 | 4:bf8f4e7c9905 | 223 | void LSM9DS0::readGyro() |
randrews33 | 4:bf8f4e7c9905 | 224 | { |
randrews33 | 0:1b975a6ae539 | 225 | uint16_t Temp = 0; |
randrews33 | 0:1b975a6ae539 | 226 | |
randrews33 | 0:1b975a6ae539 | 227 | //Get x |
randrews33 | 4:bf8f4e7c9905 | 228 | Temp = gReadByte(OUT_X_H_G); |
randrews33 | 0:1b975a6ae539 | 229 | Temp = Temp<<8; |
randrews33 | 4:bf8f4e7c9905 | 230 | Temp |= gReadByte(OUT_X_L_G); |
randrews33 | 0:1b975a6ae539 | 231 | gx = Temp; |
randrews33 | 0:1b975a6ae539 | 232 | |
randrews33 | 0:1b975a6ae539 | 233 | |
randrews33 | 0:1b975a6ae539 | 234 | //Get y |
randrews33 | 0:1b975a6ae539 | 235 | Temp=0; |
randrews33 | 4:bf8f4e7c9905 | 236 | Temp = gReadByte(OUT_Y_H_G); |
randrews33 | 0:1b975a6ae539 | 237 | Temp = Temp<<8; |
randrews33 | 4:bf8f4e7c9905 | 238 | Temp |= gReadByte(OUT_Y_L_G); |
randrews33 | 0:1b975a6ae539 | 239 | gy = Temp; |
randrews33 | 0:1b975a6ae539 | 240 | |
randrews33 | 0:1b975a6ae539 | 241 | //Get z |
randrews33 | 0:1b975a6ae539 | 242 | Temp=0; |
randrews33 | 4:bf8f4e7c9905 | 243 | Temp = gReadByte(OUT_Z_H_G); |
randrews33 | 0:1b975a6ae539 | 244 | Temp = Temp<<8; |
randrews33 | 4:bf8f4e7c9905 | 245 | Temp |= gReadByte(OUT_Z_L_G); |
randrews33 | 0:1b975a6ae539 | 246 | gz = Temp; |
randrews33 | 0:1b975a6ae539 | 247 | } |
randrews33 | 0:1b975a6ae539 | 248 | |
randrews33 | 0:1b975a6ae539 | 249 | float LSM9DS0::calcGyro(int16_t gyro) |
randrews33 | 0:1b975a6ae539 | 250 | { |
randrews33 | 0:1b975a6ae539 | 251 | // Return the gyro raw reading times our pre-calculated DPS / (ADC tick): |
randrews33 | 0:1b975a6ae539 | 252 | return gRes * gyro; |
randrews33 | 0:1b975a6ae539 | 253 | } |
randrews33 | 0:1b975a6ae539 | 254 | |
randrews33 | 0:1b975a6ae539 | 255 | float LSM9DS0::calcAccel(int16_t accel) |
randrews33 | 0:1b975a6ae539 | 256 | { |
randrews33 | 0:1b975a6ae539 | 257 | // Return the accel raw reading times our pre-calculated g's / (ADC tick): |
randrews33 | 0:1b975a6ae539 | 258 | return aRes * accel; |
randrews33 | 0:1b975a6ae539 | 259 | } |
randrews33 | 0:1b975a6ae539 | 260 | |
randrews33 | 0:1b975a6ae539 | 261 | float LSM9DS0::calcMag(int16_t mag) |
randrews33 | 0:1b975a6ae539 | 262 | { |
randrews33 | 0:1b975a6ae539 | 263 | // Return the mag raw reading times our pre-calculated Gs / (ADC tick): |
randrews33 | 0:1b975a6ae539 | 264 | return mRes * mag; |
randrews33 | 0:1b975a6ae539 | 265 | } |
randrews33 | 0:1b975a6ae539 | 266 | |
randrews33 | 0:1b975a6ae539 | 267 | void LSM9DS0::setGyroScale(gyro_scale gScl) |
randrews33 | 0:1b975a6ae539 | 268 | { |
randrews33 | 0:1b975a6ae539 | 269 | // We need to preserve the other bytes in CTRL_REG4_G. So, first read it: |
randrews33 | 0:1b975a6ae539 | 270 | uint8_t temp = gReadByte(CTRL_REG4_G); |
randrews33 | 0:1b975a6ae539 | 271 | // Then mask out the gyro scale bits: |
randrews33 | 0:1b975a6ae539 | 272 | temp &= 0xFF^(0x3 << 4); |
randrews33 | 0:1b975a6ae539 | 273 | // Then shift in our new scale bits: |
randrews33 | 0:1b975a6ae539 | 274 | temp |= gScl << 4; |
randrews33 | 0:1b975a6ae539 | 275 | // And write the new register value back into CTRL_REG4_G: |
randrews33 | 0:1b975a6ae539 | 276 | gWriteByte(CTRL_REG4_G, temp); |
randrews33 | 0:1b975a6ae539 | 277 | |
randrews33 | 0:1b975a6ae539 | 278 | // We've updated the sensor, but we also need to update our class variables |
randrews33 | 0:1b975a6ae539 | 279 | // First update gScale: |
randrews33 | 0:1b975a6ae539 | 280 | gScale = gScl; |
randrews33 | 0:1b975a6ae539 | 281 | // Then calculate a new gRes, which relies on gScale being set correctly: |
randrews33 | 0:1b975a6ae539 | 282 | calcgRes(); |
randrews33 | 0:1b975a6ae539 | 283 | } |
randrews33 | 0:1b975a6ae539 | 284 | |
randrews33 | 0:1b975a6ae539 | 285 | void LSM9DS0::setAccelScale(accel_scale aScl) |
randrews33 | 0:1b975a6ae539 | 286 | { |
randrews33 | 0:1b975a6ae539 | 287 | // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it: |
randrews33 | 0:1b975a6ae539 | 288 | uint8_t temp = xmReadByte(CTRL_REG2_XM); |
randrews33 | 0:1b975a6ae539 | 289 | // Then mask out the accel scale bits: |
randrews33 | 0:1b975a6ae539 | 290 | temp &= 0xFF^(0x3 << 3); |
randrews33 | 0:1b975a6ae539 | 291 | // Then shift in our new scale bits: |
randrews33 | 0:1b975a6ae539 | 292 | temp |= aScl << 3; |
randrews33 | 0:1b975a6ae539 | 293 | // And write the new register value back into CTRL_REG2_XM: |
randrews33 | 0:1b975a6ae539 | 294 | xmWriteByte(CTRL_REG2_XM, temp); |
randrews33 | 0:1b975a6ae539 | 295 | |
randrews33 | 0:1b975a6ae539 | 296 | // We've updated the sensor, but we also need to update our class variables |
randrews33 | 0:1b975a6ae539 | 297 | // First update aScale: |
randrews33 | 0:1b975a6ae539 | 298 | aScale = aScl; |
randrews33 | 0:1b975a6ae539 | 299 | // Then calculate a new aRes, which relies on aScale being set correctly: |
randrews33 | 0:1b975a6ae539 | 300 | calcaRes(); |
randrews33 | 0:1b975a6ae539 | 301 | } |
randrews33 | 0:1b975a6ae539 | 302 | |
randrews33 | 0:1b975a6ae539 | 303 | void LSM9DS0::setMagScale(mag_scale mScl) |
randrews33 | 0:1b975a6ae539 | 304 | { |
randrews33 | 0:1b975a6ae539 | 305 | // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it: |
randrews33 | 0:1b975a6ae539 | 306 | uint8_t temp = xmReadByte(CTRL_REG6_XM); |
randrews33 | 0:1b975a6ae539 | 307 | // Then mask out the mag scale bits: |
randrews33 | 0:1b975a6ae539 | 308 | temp &= 0xFF^(0x3 << 5); |
randrews33 | 0:1b975a6ae539 | 309 | // Then shift in our new scale bits: |
randrews33 | 0:1b975a6ae539 | 310 | temp |= mScl << 5; |
randrews33 | 0:1b975a6ae539 | 311 | // And write the new register value back into CTRL_REG6_XM: |
randrews33 | 0:1b975a6ae539 | 312 | xmWriteByte(CTRL_REG6_XM, temp); |
randrews33 | 0:1b975a6ae539 | 313 | |
randrews33 | 0:1b975a6ae539 | 314 | // We've updated the sensor, but we also need to update our class variables |
randrews33 | 0:1b975a6ae539 | 315 | // First update mScale: |
randrews33 | 0:1b975a6ae539 | 316 | mScale = mScl; |
randrews33 | 0:1b975a6ae539 | 317 | // Then calculate a new mRes, which relies on mScale being set correctly: |
randrews33 | 0:1b975a6ae539 | 318 | calcmRes(); |
randrews33 | 0:1b975a6ae539 | 319 | } |
randrews33 | 0:1b975a6ae539 | 320 | |
randrews33 | 0:1b975a6ae539 | 321 | void LSM9DS0::setGyroODR(gyro_odr gRate) |
randrews33 | 0:1b975a6ae539 | 322 | { |
randrews33 | 0:1b975a6ae539 | 323 | // We need to preserve the other bytes in CTRL_REG1_G. So, first read it: |
randrews33 | 0:1b975a6ae539 | 324 | uint8_t temp = gReadByte(CTRL_REG1_G); |
randrews33 | 0:1b975a6ae539 | 325 | // Then mask out the gyro ODR bits: |
randrews33 | 0:1b975a6ae539 | 326 | temp &= 0xFF^(0xF << 4); |
randrews33 | 0:1b975a6ae539 | 327 | // Then shift in our new ODR bits: |
randrews33 | 0:1b975a6ae539 | 328 | temp |= (gRate << 4); |
randrews33 | 0:1b975a6ae539 | 329 | // And write the new register value back into CTRL_REG1_G: |
randrews33 | 0:1b975a6ae539 | 330 | gWriteByte(CTRL_REG1_G, temp); |
randrews33 | 0:1b975a6ae539 | 331 | } |
randrews33 | 0:1b975a6ae539 | 332 | void LSM9DS0::setAccelODR(accel_odr aRate) |
randrews33 | 0:1b975a6ae539 | 333 | { |
randrews33 | 0:1b975a6ae539 | 334 | // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it: |
randrews33 | 0:1b975a6ae539 | 335 | uint8_t temp = xmReadByte(CTRL_REG1_XM); |
randrews33 | 0:1b975a6ae539 | 336 | // Then mask out the accel ODR bits: |
randrews33 | 0:1b975a6ae539 | 337 | temp &= 0xFF^(0xF << 4); |
randrews33 | 0:1b975a6ae539 | 338 | // Then shift in our new ODR bits: |
randrews33 | 0:1b975a6ae539 | 339 | temp |= (aRate << 4); |
randrews33 | 0:1b975a6ae539 | 340 | // And write the new register value back into CTRL_REG1_XM: |
randrews33 | 0:1b975a6ae539 | 341 | xmWriteByte(CTRL_REG1_XM, temp); |
randrews33 | 0:1b975a6ae539 | 342 | } |
randrews33 | 0:1b975a6ae539 | 343 | void LSM9DS0::setMagODR(mag_odr mRate) |
randrews33 | 0:1b975a6ae539 | 344 | { |
randrews33 | 0:1b975a6ae539 | 345 | // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it: |
randrews33 | 0:1b975a6ae539 | 346 | uint8_t temp = xmReadByte(CTRL_REG5_XM); |
randrews33 | 0:1b975a6ae539 | 347 | // Then mask out the mag ODR bits: |
randrews33 | 0:1b975a6ae539 | 348 | temp &= 0xFF^(0x7 << 2); |
randrews33 | 0:1b975a6ae539 | 349 | // Then shift in our new ODR bits: |
randrews33 | 0:1b975a6ae539 | 350 | temp |= (mRate << 2); |
randrews33 | 0:1b975a6ae539 | 351 | // And write the new register value back into CTRL_REG5_XM: |
randrews33 | 0:1b975a6ae539 | 352 | xmWriteByte(CTRL_REG5_XM, temp); |
randrews33 | 0:1b975a6ae539 | 353 | } |
randrews33 | 0:1b975a6ae539 | 354 | |
randrews33 | 0:1b975a6ae539 | 355 | void LSM9DS0::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration) |
randrews33 | 0:1b975a6ae539 | 356 | { |
randrews33 | 0:1b975a6ae539 | 357 | gWriteByte(INT1_CFG_G, int1Cfg); |
randrews33 | 0:1b975a6ae539 | 358 | gWriteByte(INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8); |
randrews33 | 0:1b975a6ae539 | 359 | gWriteByte(INT1_THS_XL_G, (int1ThsX & 0xFF)); |
randrews33 | 0:1b975a6ae539 | 360 | gWriteByte(INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8); |
randrews33 | 0:1b975a6ae539 | 361 | gWriteByte(INT1_THS_YL_G, (int1ThsY & 0xFF)); |
randrews33 | 0:1b975a6ae539 | 362 | gWriteByte(INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8); |
randrews33 | 0:1b975a6ae539 | 363 | gWriteByte(INT1_THS_ZL_G, (int1ThsZ & 0xFF)); |
randrews33 | 0:1b975a6ae539 | 364 | if (duration) |
randrews33 | 0:1b975a6ae539 | 365 | gWriteByte(INT1_DURATION_G, 0x80 | duration); |
randrews33 | 0:1b975a6ae539 | 366 | else |
randrews33 | 0:1b975a6ae539 | 367 | gWriteByte(INT1_DURATION_G, 0x00); |
randrews33 | 0:1b975a6ae539 | 368 | } |
randrews33 | 0:1b975a6ae539 | 369 | |
randrews33 | 0:1b975a6ae539 | 370 | void LSM9DS0::calcgRes() |
randrews33 | 0:1b975a6ae539 | 371 | { |
randrews33 | 0:1b975a6ae539 | 372 | // Possible gyro scales (and their register bit settings) are: |
randrews33 | 0:1b975a6ae539 | 373 | // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm |
randrews33 | 0:1b975a6ae539 | 374 | // to calculate DPS/(ADC tick) based on that 2-bit value: |
randrews33 | 0:1b975a6ae539 | 375 | switch (gScale) |
randrews33 | 0:1b975a6ae539 | 376 | { |
randrews33 | 0:1b975a6ae539 | 377 | case G_SCALE_245DPS: |
randrews33 | 0:1b975a6ae539 | 378 | gRes = 245.0 / 32768.0; |
randrews33 | 0:1b975a6ae539 | 379 | break; |
randrews33 | 0:1b975a6ae539 | 380 | case G_SCALE_500DPS: |
randrews33 | 0:1b975a6ae539 | 381 | gRes = 500.0 / 32768.0; |
randrews33 | 0:1b975a6ae539 | 382 | break; |
randrews33 | 0:1b975a6ae539 | 383 | case G_SCALE_2000DPS: |
randrews33 | 0:1b975a6ae539 | 384 | gRes = 2000.0 / 32768.0; |
randrews33 | 0:1b975a6ae539 | 385 | break; |
randrews33 | 0:1b975a6ae539 | 386 | } |
randrews33 | 0:1b975a6ae539 | 387 | } |
randrews33 | 0:1b975a6ae539 | 388 | |
randrews33 | 0:1b975a6ae539 | 389 | void LSM9DS0::calcaRes() |
randrews33 | 0:1b975a6ae539 | 390 | { |
randrews33 | 0:1b975a6ae539 | 391 | // Possible accelerometer scales (and their register bit settings) are: |
randrews33 | 0:1b975a6ae539 | 392 | // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an |
randrews33 | 0:1b975a6ae539 | 393 | // algorithm to calculate g/(ADC tick) based on that 3-bit value: |
randrews33 | 0:1b975a6ae539 | 394 | aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : |
randrews33 | 0:1b975a6ae539 | 395 | (((float) aScale + 1.0) * 2.0) / 32768.0; |
randrews33 | 0:1b975a6ae539 | 396 | } |
randrews33 | 0:1b975a6ae539 | 397 | |
randrews33 | 0:1b975a6ae539 | 398 | void LSM9DS0::calcmRes() |
randrews33 | 0:1b975a6ae539 | 399 | { |
randrews33 | 0:1b975a6ae539 | 400 | // Possible magnetometer scales (and their register bit settings) are: |
randrews33 | 0:1b975a6ae539 | 401 | // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm |
randrews33 | 0:1b975a6ae539 | 402 | // to calculate Gs/(ADC tick) based on that 2-bit value: |
randrews33 | 0:1b975a6ae539 | 403 | mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 : |
randrews33 | 0:1b975a6ae539 | 404 | (float) (mScale << 2) / 32768.0; |
randrews33 | 0:1b975a6ae539 | 405 | } |
randrews33 | 0:1b975a6ae539 | 406 | |
randrews33 | 0:1b975a6ae539 | 407 | void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data) |
randrews33 | 0:1b975a6ae539 | 408 | { |
randrews33 | 0:1b975a6ae539 | 409 | // Whether we're using I2C or SPI, write a byte using the |
randrews33 | 0:1b975a6ae539 | 410 | // gyro-specific I2C address or SPI CS pin. |
randrews33 | 0:1b975a6ae539 | 411 | I2CwriteByte(gAddress, subAddress, data); |
randrews33 | 0:1b975a6ae539 | 412 | } |
randrews33 | 0:1b975a6ae539 | 413 | |
randrews33 | 0:1b975a6ae539 | 414 | void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data) |
randrews33 | 0:1b975a6ae539 | 415 | { |
randrews33 | 0:1b975a6ae539 | 416 | // Whether we're using I2C or SPI, write a byte using the |
randrews33 | 0:1b975a6ae539 | 417 | // accelerometer-specific I2C address or SPI CS pin. |
randrews33 | 0:1b975a6ae539 | 418 | return I2CwriteByte(xmAddress, subAddress, data); |
randrews33 | 0:1b975a6ae539 | 419 | } |
randrews33 | 0:1b975a6ae539 | 420 | |
randrews33 | 0:1b975a6ae539 | 421 | uint8_t LSM9DS0::gReadByte(uint8_t subAddress) |
randrews33 | 0:1b975a6ae539 | 422 | { |
randrews33 | 0:1b975a6ae539 | 423 | return I2CreadByte(gAddress, subAddress); |
randrews33 | 0:1b975a6ae539 | 424 | } |
randrews33 | 0:1b975a6ae539 | 425 | |
randrews33 | 0:1b975a6ae539 | 426 | void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) |
randrews33 | 0:1b975a6ae539 | 427 | { |
randrews33 | 0:1b975a6ae539 | 428 | // Whether we're using I2C or SPI, read multiple bytes using the |
randrews33 | 4:bf8f4e7c9905 | 429 | // gyro-specific I2C address. |
randrews33 | 0:1b975a6ae539 | 430 | I2CreadBytes(gAddress, subAddress, dest, count); |
randrews33 | 0:1b975a6ae539 | 431 | } |
randrews33 | 0:1b975a6ae539 | 432 | |
randrews33 | 0:1b975a6ae539 | 433 | uint8_t LSM9DS0::xmReadByte(uint8_t subAddress) |
randrews33 | 0:1b975a6ae539 | 434 | { |
randrews33 | 0:1b975a6ae539 | 435 | // Whether we're using I2C or SPI, read a byte using the |
randrews33 | 4:bf8f4e7c9905 | 436 | // accelerometer-specific I2C address. |
randrews33 | 0:1b975a6ae539 | 437 | return I2CreadByte(xmAddress, subAddress); |
randrews33 | 0:1b975a6ae539 | 438 | } |
randrews33 | 0:1b975a6ae539 | 439 | |
randrews33 | 0:1b975a6ae539 | 440 | void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) |
randrews33 | 0:1b975a6ae539 | 441 | { |
randrews33 | 4:bf8f4e7c9905 | 442 | // read multiple bytes using the |
randrews33 | 4:bf8f4e7c9905 | 443 | // accelerometer-specific I2C address. |
randrews33 | 4:bf8f4e7c9905 | 444 | I2CreadBytes(xmAddress, subAddress, dest, count); |
randrews33 | 0:1b975a6ae539 | 445 | } |
randrews33 | 0:1b975a6ae539 | 446 | |
randrews33 | 0:1b975a6ae539 | 447 | |
randrews33 | 0:1b975a6ae539 | 448 | void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data) |
randrews33 | 4:bf8f4e7c9905 | 449 | { |
randrews33 | 0:1b975a6ae539 | 450 | i2c_->writeByte(address,subAddress,data); |
randrews33 | 0:1b975a6ae539 | 451 | } |
randrews33 | 0:1b975a6ae539 | 452 | |
randrews33 | 0:1b975a6ae539 | 453 | uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress) |
randrews33 | 0:1b975a6ae539 | 454 | { |
randrews33 | 0:1b975a6ae539 | 455 | char data[1]; // `data` will store the register data |
randrews33 | 0:1b975a6ae539 | 456 | |
randrews33 | 0:1b975a6ae539 | 457 | I2CreadBytes(address, subAddress,(uint8_t*)data, 1); |
randrews33 | 0:1b975a6ae539 | 458 | return (uint8_t)data[0]; |
randrews33 | 0:1b975a6ae539 | 459 | |
randrews33 | 0:1b975a6ae539 | 460 | } |
randrews33 | 0:1b975a6ae539 | 461 | |
randrews33 | 0:1b975a6ae539 | 462 | void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, |
randrews33 | 0:1b975a6ae539 | 463 | uint8_t count) |
randrews33 | 4:bf8f4e7c9905 | 464 | { |
randrews33 | 0:1b975a6ae539 | 465 | i2c_->readBytes(address, subAddress, count, dest); |
randrews33 | 0:1b975a6ae539 | 466 | } |