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