Raynaud Gilles / LSM9DS1

VGR

Dependents:   VITI_motor_angle_1 VITI_motor_angle_2 VITI_motor_angle_3

LSM9DS1.cpp@6:28c4b3c8b43d, 2017-06-15 (annotated)

Committer:
ChangYuHsuan
Date:
Thu Jun 15 07:17:11 2017 +0000
Revision:
6:28c4b3c8b43d
Parent:
4:7ffcb378cfd4
Child:
7:e6e3d320eb6c
revise

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beanmachine44 0:622e8874902e 1 #include "LSM9DS1.h"
beanmachine44 0:622e8874902e 2
beanmachine44 0:622e8874902e 3 LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr) : i2c(sda, scl)
beanmachine44 0:622e8874902e 4 {
beanmachine44 0:622e8874902e 5 // xgAddress and mAddress will store the 7-bit I2C address, if using I2C.
beanmachine44 0:622e8874902e 6 xgAddress = xgAddr;
beanmachine44 0:622e8874902e 7 mAddress = mAddr;
beanmachine44 0:622e8874902e 8 }
beanmachine44 0:622e8874902e 9
beanmachine44 0:622e8874902e 10 uint16_t LSM9DS1::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
beanmachine44 0:622e8874902e 11 gyro_odr gODR, accel_odr aODR, mag_odr mODR)
beanmachine44 0:622e8874902e 12 {
beanmachine44 0:622e8874902e 13 // Store the given scales in class variables. These scale variables
beanmachine44 0:622e8874902e 14 // are used throughout to calculate the actual g's, DPS,and Gs's.
beanmachine44 0:622e8874902e 15 gScale = gScl;
beanmachine44 0:622e8874902e 16 aScale = aScl;
beanmachine44 0:622e8874902e 17 mScale = mScl;
beanmachine44 0:622e8874902e 18
beanmachine44 0:622e8874902e 19 // Once we have the scale values, we can calculate the resolution
beanmachine44 0:622e8874902e 20 // of each sensor. That's what these functions are for. One for each sensor
beanmachine44 0:622e8874902e 21 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
beanmachine44 0:622e8874902e 22 calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
beanmachine44 0:622e8874902e 23 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
beanmachine44 0:622e8874902e 24
beanmachine44 0:622e8874902e 25
beanmachine44 0:622e8874902e 26 // To verify communication, we can read from the WHO_AM_I register of
beanmachine44 0:622e8874902e 27 // each device. Store those in a variable so we can return them.
beanmachine44 0:622e8874902e 28 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 29 char cmd[2] = {
beanmachine44 0:622e8874902e 30 WHO_AM_I_XG,
beanmachine44 0:622e8874902e 31 0
beanmachine44 0:622e8874902e 32 };
beanmachine44 0:622e8874902e 33
beanmachine44 0:622e8874902e 34 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 35 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 36 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 37 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 38 uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I
beanmachine44 0:622e8874902e 39
beanmachine44 0:622e8874902e 40 // Reset to the address of the mag who am i
ChangYuHsuan 6:28c4b3c8b43d 41 cmd[0] = WHO_AM_I_M;
beanmachine44 0:622e8874902e 42 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 43 i2c.write(mAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 44 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 45 i2c.read(mAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 46 uint8_t mTest = cmd[1]; // Read the mag WHO_AM_I
beanmachine44 0:622e8874902e 47
ChangYuHsuan 6:28c4b3c8b43d 48 for(int ii = 0; ii < 3; ii++)
ChangYuHsuan 6:28c4b3c8b43d 49 {
ChangYuHsuan 6:28c4b3c8b43d 50 gBiasRaw[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 51 aBiasRaw[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 52 gBias[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 53 aBias[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 54 autoCalib = false;
ChangYuHsuan 6:28c4b3c8b43d 55 }
ChangYuHsuan 6:28c4b3c8b43d 56
beanmachine44 0:622e8874902e 57 // Gyro initialization stuff:
beanmachine44 0:622e8874902e 58 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
beanmachine44 0:622e8874902e 59 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
beanmachine44 0:622e8874902e 60 setGyroScale(gScale); // Set the gyro range
beanmachine44 0:622e8874902e 61
beanmachine44 0:622e8874902e 62 // Accelerometer initialization stuff:
beanmachine44 0:622e8874902e 63 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
beanmachine44 0:622e8874902e 64 setAccelODR(aODR); // Set the accel data rate.
beanmachine44 0:622e8874902e 65 setAccelScale(aScale); // Set the accel range.
beanmachine44 0:622e8874902e 66
beanmachine44 0:622e8874902e 67 // Magnetometer initialization stuff:
beanmachine44 0:622e8874902e 68 initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
beanmachine44 0:622e8874902e 69 setMagODR(mODR); // Set the magnetometer output data rate.
beanmachine44 0:622e8874902e 70 setMagScale(mScale); // Set the magnetometer's range.
beanmachine44 0:622e8874902e 71
5hel2l2y 4:7ffcb378cfd4 72 // Interrupt initialization stuff
5hel2l2y 3:f96b287c0bf7 73 initIntr();
5hel2l2y 3:f96b287c0bf7 74
beanmachine44 0:622e8874902e 75 // Once everything is initialized, return the WHO_AM_I registers we read:
beanmachine44 0:622e8874902e 76 return (xgTest << 8) | mTest;
beanmachine44 0:622e8874902e 77 }
beanmachine44 0:622e8874902e 78
beanmachine44 0:622e8874902e 79 void LSM9DS1::initGyro()
beanmachine44 0:622e8874902e 80 {
beanmachine44 0:622e8874902e 81 char cmd[4] = {
beanmachine44 0:622e8874902e 82 CTRL_REG1_G,
beanmachine44 0:622e8874902e 83 gScale | G_ODR_119_BW_14,
beanmachine44 0:622e8874902e 84 0, // Default data out and int out
beanmachine44 0:622e8874902e 85 0 // Default power mode and high pass settings
beanmachine44 0:622e8874902e 86 };
beanmachine44 0:622e8874902e 87
beanmachine44 0:622e8874902e 88 // Write the data to the gyro control registers
beanmachine44 0:622e8874902e 89 i2c.write(xgAddress, cmd, 4);
beanmachine44 0:622e8874902e 90 }
beanmachine44 0:622e8874902e 91
beanmachine44 0:622e8874902e 92 void LSM9DS1::initAccel()
beanmachine44 0:622e8874902e 93 {
beanmachine44 0:622e8874902e 94 char cmd[4] = {
beanmachine44 0:622e8874902e 95 CTRL_REG5_XL,
beanmachine44 0:622e8874902e 96 0x38, // Enable all axis and don't decimate data in out Registers
beanmachine44 0:622e8874902e 97 (A_ODR_119 << 5) | (aScale << 3) | (A_BW_AUTO_SCALE), // 119 Hz ODR, set scale, and auto BW
beanmachine44 0:622e8874902e 98 0 // Default resolution mode and filtering settings
beanmachine44 0:622e8874902e 99 };
beanmachine44 0:622e8874902e 100
beanmachine44 0:622e8874902e 101 // Write the data to the accel control registers
beanmachine44 0:622e8874902e 102 i2c.write(xgAddress, cmd, 4);
beanmachine44 0:622e8874902e 103 }
beanmachine44 0:622e8874902e 104
beanmachine44 0:622e8874902e 105 void LSM9DS1::initMag()
beanmachine44 0:622e8874902e 106 {
beanmachine44 0:622e8874902e 107 char cmd[4] = {
beanmachine44 0:622e8874902e 108 CTRL_REG1_M,
beanmachine44 0:622e8874902e 109 0x10, // Default data rate, xy axes mode, and temp comp
beanmachine44 0:622e8874902e 110 mScale << 5, // Set mag scale
beanmachine44 0:622e8874902e 111 0 // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
beanmachine44 0:622e8874902e 112 };
beanmachine44 0:622e8874902e 113
beanmachine44 0:622e8874902e 114 // Write the data to the mag control registers
beanmachine44 0:622e8874902e 115 i2c.write(mAddress, cmd, 4);
beanmachine44 0:622e8874902e 116 }
beanmachine44 0:622e8874902e 117
5hel2l2y 3:f96b287c0bf7 118 void LSM9DS1::initIntr()
5hel2l2y 3:f96b287c0bf7 119 {
5hel2l2y 3:f96b287c0bf7 120 char cmd[2];
5hel2l2y 4:7ffcb378cfd4 121 uint16_t thresholdG = 500;
5hel2l2y 4:7ffcb378cfd4 122 uint8_t durationG = 10;
5hel2l2y 4:7ffcb378cfd4 123 uint8_t thresholdX = 20;
5hel2l2y 4:7ffcb378cfd4 124 uint8_t durationX = 1;
5hel2l2y 4:7ffcb378cfd4 125 uint16_t thresholdM = 10000;
5hel2l2y 3:f96b287c0bf7 126
5hel2l2y 4:7ffcb378cfd4 127 // 1. Configure the gyro interrupt generator
5hel2l2y 3:f96b287c0bf7 128 cmd[0] = INT_GEN_CFG_G;
5hel2l2y 3:f96b287c0bf7 129 cmd[1] = (1 << 5);
5hel2l2y 3:f96b287c0bf7 130 i2c.write(xgAddress, cmd, 2);
5hel2l2y 4:7ffcb378cfd4 131 // 2. Configure the gyro threshold
5hel2l2y 4:7ffcb378cfd4 132 cmd[0] = INT_GEN_THS_ZH_G;
5hel2l2y 4:7ffcb378cfd4 133 cmd[1] = (thresholdG & 0x7F00) >> 8;
5hel2l2y 3:f96b287c0bf7 134 i2c.write(xgAddress, cmd, 2);
5hel2l2y 4:7ffcb378cfd4 135 cmd[0] = INT_GEN_THS_ZL_G;
5hel2l2y 4:7ffcb378cfd4 136 cmd[1] = (thresholdG & 0x00FF);
5hel2l2y 3:f96b287c0bf7 137 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 138 cmd[0] = INT_GEN_DUR_G;
5hel2l2y 4:7ffcb378cfd4 139 cmd[1] = (durationG & 0x7F) | 0x80;
5hel2l2y 3:f96b287c0bf7 140 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 141
5hel2l2y 4:7ffcb378cfd4 142 // 3. Configure accelerometer interrupt generator
5hel2l2y 3:f96b287c0bf7 143 cmd[0] = INT_GEN_CFG_XL;
5hel2l2y 3:f96b287c0bf7 144 cmd[1] = (1 << 1);
5hel2l2y 3:f96b287c0bf7 145 i2c.write(xgAddress, cmd, 2);
5hel2l2y 4:7ffcb378cfd4 146 // 4. Configure accelerometer threshold
5hel2l2y 4:7ffcb378cfd4 147 cmd[0] = INT_GEN_THS_X_XL;
5hel2l2y 4:7ffcb378cfd4 148 cmd[1] = thresholdX;
5hel2l2y 3:f96b287c0bf7 149 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 150 cmd[0] = INT_GEN_DUR_XL;
5hel2l2y 4:7ffcb378cfd4 151 cmd[1] = (durationX & 0x7F);
5hel2l2y 3:f96b287c0bf7 152 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 153
5hel2l2y 4:7ffcb378cfd4 154 // 5. Configure INT1 - assign it to gyro interrupt
5hel2l2y 3:f96b287c0bf7 155 cmd[0] = INT1_CTRL;
5hel2l2y 4:7ffcb378cfd4 156 // cmd[1] = 0xC0;
5hel2l2y 3:f96b287c0bf7 157 cmd[1] = (1 << 7) | (1 << 6);
5hel2l2y 3:f96b287c0bf7 158 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 159 cmd[0] = CTRL_REG8;
5hel2l2y 4:7ffcb378cfd4 160 // cmd[1] = 0x04;
5hel2l2y 4:7ffcb378cfd4 161 cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
5hel2l2y 3:f96b287c0bf7 162 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 163
5hel2l2y 4:7ffcb378cfd4 164 // Configure interrupt 2 to fire whenever new accelerometer
5hel2l2y 4:7ffcb378cfd4 165 // or gyroscope data is available.
5hel2l2y 3:f96b287c0bf7 166 cmd[0] = INT2_CTRL;
5hel2l2y 3:f96b287c0bf7 167 cmd[1] = (1 << 0) | (1 << 1);
5hel2l2y 3:f96b287c0bf7 168 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 169 cmd[0] = CTRL_REG8;
5hel2l2y 4:7ffcb378cfd4 170 cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
5hel2l2y 3:f96b287c0bf7 171 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 172
5hel2l2y 4:7ffcb378cfd4 173 // Configure magnetometer interrupt
5hel2l2y 3:f96b287c0bf7 174 cmd[0] = INT_CFG_M;
5hel2l2y 4:7ffcb378cfd4 175 cmd[1] = (1 << 7) | (1 << 0);
5hel2l2y 3:f96b287c0bf7 176 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 177
5hel2l2y 4:7ffcb378cfd4 178 // Configure magnetometer threshold
5hel2l2y 3:f96b287c0bf7 179 cmd[0] = INT_THS_H_M;
5hel2l2y 4:7ffcb378cfd4 180 cmd[1] = uint8_t((thresholdM & 0x7F00) >> 8);
5hel2l2y 3:f96b287c0bf7 181 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 182 cmd[0] = INT_THS_L_M;
5hel2l2y 4:7ffcb378cfd4 183 cmd[1] = uint8_t(thresholdM & 0x00FF);
5hel2l2y 3:f96b287c0bf7 184 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 185 }
5hel2l2y 3:f96b287c0bf7 186
ChangYuHsuan 6:28c4b3c8b43d 187 void LSM9DS1::calibration()
ChangYuHsuan 6:28c4b3c8b43d 188 {
ChangYuHsuan 6:28c4b3c8b43d 189
ChangYuHsuan 6:28c4b3c8b43d 190 uint16_t samples = 0;
ChangYuHsuan 6:28c4b3c8b43d 191 int32_t aBiasRawTemp[3] = {0, 0, 0};
ChangYuHsuan 6:28c4b3c8b43d 192 int32_t gBiasRawTemp[3] = {0, 0, 0};
ChangYuHsuan 6:28c4b3c8b43d 193 /*
ChangYuHsuan 6:28c4b3c8b43d 194 // Turn on FIFO and set threshold to 32 samples
ChangYuHsuan 6:28c4b3c8b43d 195 enableXgFIFO(true);
ChangYuHsuan 6:28c4b3c8b43d 196 setXgFIFO( 1, 0x1F);
ChangYuHsuan 6:28c4b3c8b43d 197 while (samples < 0x1F)
ChangYuHsuan 6:28c4b3c8b43d 198 {
ChangYuHsuan 6:28c4b3c8b43d 199 samples = (i2c.read(FIFO_SRC) & 0x3F); // Read number of stored samples
ChangYuHsuan 6:28c4b3c8b43d 200 }
ChangYuHsuan 6:28c4b3c8b43d 201 for(int ii = 0; ii < samples ; ii++)
ChangYuHsuan 6:28c4b3c8b43d 202 { // Read the gyro data stored in the FIFO
ChangYuHsuan 6:28c4b3c8b43d 203 readGyro();
ChangYuHsuan 6:28c4b3c8b43d 204 gBiasRawTemp[0] += gx_raw;
ChangYuHsuan 6:28c4b3c8b43d 205 gBiasRawTemp[1] += gy_raw;
ChangYuHsuan 6:28c4b3c8b43d 206 gBiasRawTemp[2] += gz_raw;
ChangYuHsuan 6:28c4b3c8b43d 207 readAccel();
ChangYuHsuan 6:28c4b3c8b43d 208 aBiasRawTemp[0] += ax_raw;
ChangYuHsuan 6:28c4b3c8b43d 209 aBiasRawTemp[1] += ay_raw;
ChangYuHsuan 6:28c4b3c8b43d 210 aBiasRawTemp[2] += az_raw - (int16_t)(1./aRes); // Assumes sensor facing up!
ChangYuHsuan 6:28c4b3c8b43d 211 }
ChangYuHsuan 6:28c4b3c8b43d 212 for (int ii = 0; ii < 3; ii++)
ChangYuHsuan 6:28c4b3c8b43d 213 {
ChangYuHsuan 6:28c4b3c8b43d 214 gBias_raw[ii] = gBiasRawTemp[ii] / samples;
ChangYuHsuan 6:28c4b3c8b43d 215 gBias[ii] = gBias_raw[ii] * gRes;
ChangYuHsuan 6:28c4b3c8b43d 216 aBias_raw[ii] = aBiasRawTemp[ii] / samples;
ChangYuHsuan 6:28c4b3c8b43d 217 aBias[ii] = aBias_raw[ii] * aRes;
ChangYuHsuan 6:28c4b3c8b43d 218 }
ChangYuHsuan 6:28c4b3c8b43d 219
ChangYuHsuan 6:28c4b3c8b43d 220
ChangYuHsuan 6:28c4b3c8b43d 221
ChangYuHsuan 6:28c4b3c8b43d 222 enableXgFIFO(false);
ChangYuHsuan 6:28c4b3c8b43d 223 setXgFIFO(0, 0x00);
ChangYuHsuan 6:28c4b3c8b43d 224 */
ChangYuHsuan 6:28c4b3c8b43d 225 while(samples < 300)
ChangYuHsuan 6:28c4b3c8b43d 226 {
ChangYuHsuan 6:28c4b3c8b43d 227 readGyro();
ChangYuHsuan 6:28c4b3c8b43d 228 gBiasRawTemp[0] += gx_raw;
ChangYuHsuan 6:28c4b3c8b43d 229 gBiasRawTemp[1] += gy_raw;
ChangYuHsuan 6:28c4b3c8b43d 230 gBiasRawTemp[2] += gz_raw;
ChangYuHsuan 6:28c4b3c8b43d 231 readAccel();
ChangYuHsuan 6:28c4b3c8b43d 232 aBiasRawTemp[0] += ax_raw;
ChangYuHsuan 6:28c4b3c8b43d 233 aBiasRawTemp[1] += ay_raw;
ChangYuHsuan 6:28c4b3c8b43d 234 aBiasRawTemp[2] += az_raw;
ChangYuHsuan 6:28c4b3c8b43d 235 wait_us(1000);
ChangYuHsuan 6:28c4b3c8b43d 236 samples++;
ChangYuHsuan 6:28c4b3c8b43d 237 }
ChangYuHsuan 6:28c4b3c8b43d 238
ChangYuHsuan 6:28c4b3c8b43d 239 for(int ii = 0; ii < 3; ii++)
ChangYuHsuan 6:28c4b3c8b43d 240 {
ChangYuHsuan 6:28c4b3c8b43d 241 gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
ChangYuHsuan 6:28c4b3c8b43d 242 aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
ChangYuHsuan 6:28c4b3c8b43d 243
ChangYuHsuan 6:28c4b3c8b43d 244 gBias[ii] = gBiasRaw[ii] * gRes;
ChangYuHsuan 6:28c4b3c8b43d 245 aBias[ii] = aBiasRaw[ii] * aRes;
ChangYuHsuan 6:28c4b3c8b43d 246 }
ChangYuHsuan 6:28c4b3c8b43d 247
ChangYuHsuan 6:28c4b3c8b43d 248 autoCalib = true;
ChangYuHsuan 6:28c4b3c8b43d 249 }
ChangYuHsuan 6:28c4b3c8b43d 250
beanmachine44 0:622e8874902e 251 void LSM9DS1::readAccel()
beanmachine44 0:622e8874902e 252 {
beanmachine44 0:622e8874902e 253 // The data we are going to read from the accel
beanmachine44 0:622e8874902e 254 char data[6];
beanmachine44 0:622e8874902e 255
beanmachine44 0:622e8874902e 256 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 257 char subAddress = OUT_X_L_XL;
beanmachine44 0:622e8874902e 258
beanmachine44 0:622e8874902e 259 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 260 i2c.write(xgAddress, &subAddress, 1, true);
beanmachine44 0:622e8874902e 261 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 262 i2c.read(xgAddress, data, 6);
beanmachine44 0:622e8874902e 263
beanmachine44 0:622e8874902e 264 // Reassemble the data and convert to g
beanmachine44 0:622e8874902e 265 ax_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 266 ay_raw = data[2] | (data[3] << 8);
beanmachine44 0:622e8874902e 267 az_raw = data[4] | (data[5] << 8);
beanmachine44 0:622e8874902e 268 ax = ax_raw * aRes;
beanmachine44 0:622e8874902e 269 ay = ay_raw * aRes;
beanmachine44 0:622e8874902e 270 az = az_raw * aRes;
ChangYuHsuan 6:28c4b3c8b43d 271
ChangYuHsuan 6:28c4b3c8b43d 272 if(autoCalib == true)
ChangYuHsuan 6:28c4b3c8b43d 273 {
ChangYuHsuan 6:28c4b3c8b43d 274 ax_raw -= aBiasRaw[0];
ChangYuHsuan 6:28c4b3c8b43d 275 ay_raw -= aBiasRaw[1];
ChangYuHsuan 6:28c4b3c8b43d 276 az_raw -= aBiasRaw[2];
ChangYuHsuan 6:28c4b3c8b43d 277
ChangYuHsuan 6:28c4b3c8b43d 278 ax = ax_raw * aRes;
ChangYuHsuan 6:28c4b3c8b43d 279 ay = ay_raw * aRes;
ChangYuHsuan 6:28c4b3c8b43d 280 az = az_raw * aRes;
ChangYuHsuan 6:28c4b3c8b43d 281 }
beanmachine44 0:622e8874902e 282 }
beanmachine44 0:622e8874902e 283
beanmachine44 0:622e8874902e 284 void LSM9DS1::readMag()
beanmachine44 0:622e8874902e 285 {
beanmachine44 0:622e8874902e 286 // The data we are going to read from the mag
beanmachine44 0:622e8874902e 287 char data[6];
beanmachine44 0:622e8874902e 288
beanmachine44 0:622e8874902e 289 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 290 char subAddress = OUT_X_L_M;
beanmachine44 0:622e8874902e 291
beanmachine44 0:622e8874902e 292 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 293 i2c.write(mAddress, &subAddress, 1, true);
beanmachine44 0:622e8874902e 294 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 295 i2c.read(mAddress, data, 6);
beanmachine44 0:622e8874902e 296
beanmachine44 0:622e8874902e 297 // Reassemble the data and convert to degrees
beanmachine44 0:622e8874902e 298 mx_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 299 my_raw = data[2] | (data[3] << 8);
beanmachine44 0:622e8874902e 300 mz_raw = data[4] | (data[5] << 8);
beanmachine44 0:622e8874902e 301 mx = mx_raw * mRes;
beanmachine44 0:622e8874902e 302 my = my_raw * mRes;
beanmachine44 0:622e8874902e 303 mz = mz_raw * mRes;
beanmachine44 0:622e8874902e 304 }
beanmachine44 0:622e8874902e 305
5hel2l2y 3:f96b287c0bf7 306 void LSM9DS1::readIntr()
5hel2l2y 3:f96b287c0bf7 307 {
5hel2l2y 3:f96b287c0bf7 308 char data[1];
5hel2l2y 3:f96b287c0bf7 309 char subAddress = INT_GEN_SRC_G;
5hel2l2y 3:f96b287c0bf7 310
5hel2l2y 3:f96b287c0bf7 311 i2c.write(xgAddress, &subAddress, 1, true);
5hel2l2y 3:f96b287c0bf7 312 i2c.read(xgAddress, data, 1);
5hel2l2y 3:f96b287c0bf7 313
5hel2l2y 3:f96b287c0bf7 314 intr = (float)data[0];
5hel2l2y 3:f96b287c0bf7 315 }
5hel2l2y 3:f96b287c0bf7 316
beanmachine44 0:622e8874902e 317 void LSM9DS1::readTemp()
beanmachine44 0:622e8874902e 318 {
beanmachine44 0:622e8874902e 319 // The data we are going to read from the temp
beanmachine44 0:622e8874902e 320 char data[2];
beanmachine44 0:622e8874902e 321
beanmachine44 0:622e8874902e 322 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 323 char subAddress = OUT_TEMP_L;
beanmachine44 0:622e8874902e 324
beanmachine44 0:622e8874902e 325 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 326 i2c.write(xgAddress, &subAddress, 1, true);
beanmachine44 0:622e8874902e 327 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 328 i2c.read(xgAddress, data, 2);
beanmachine44 0:622e8874902e 329
beanmachine44 0:622e8874902e 330 // Temperature is a 12-bit signed integer
beanmachine44 0:622e8874902e 331 temperature_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 332
beanmachine44 0:622e8874902e 333 temperature_c = (float)temperature_raw / 8.0 + 25;
beanmachine44 0:622e8874902e 334 temperature_f = temperature_c * 1.8 + 32;
beanmachine44 0:622e8874902e 335 }
beanmachine44 0:622e8874902e 336
beanmachine44 0:622e8874902e 337 void LSM9DS1::readGyro()
beanmachine44 0:622e8874902e 338 {
beanmachine44 0:622e8874902e 339 // The data we are going to read from the gyro
beanmachine44 0:622e8874902e 340 char data[6];
beanmachine44 0:622e8874902e 341
beanmachine44 0:622e8874902e 342 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 343 char subAddress = OUT_X_L_G;
beanmachine44 0:622e8874902e 344
beanmachine44 0:622e8874902e 345 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 346 i2c.write(xgAddress, &subAddress, 1, true);
beanmachine44 0:622e8874902e 347 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 348 i2c.read(xgAddress, data, 6);
beanmachine44 0:622e8874902e 349
beanmachine44 0:622e8874902e 350 // Reassemble the data and convert to degrees/sec
beanmachine44 0:622e8874902e 351 gx_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 352 gy_raw = data[2] | (data[3] << 8);
beanmachine44 0:622e8874902e 353 gz_raw = data[4] | (data[5] << 8);
beanmachine44 0:622e8874902e 354 gx = gx_raw * gRes;
beanmachine44 0:622e8874902e 355 gy = gy_raw * gRes;
beanmachine44 0:622e8874902e 356 gz = gz_raw * gRes;
ChangYuHsuan 6:28c4b3c8b43d 357
ChangYuHsuan 6:28c4b3c8b43d 358 if(autoCalib == true)
ChangYuHsuan 6:28c4b3c8b43d 359 {
ChangYuHsuan 6:28c4b3c8b43d 360 gx_raw -= gBiasRaw[0];
ChangYuHsuan 6:28c4b3c8b43d 361 gy_raw -= gBiasRaw[1];
ChangYuHsuan 6:28c4b3c8b43d 362 gz_raw -= gBiasRaw[2];
ChangYuHsuan 6:28c4b3c8b43d 363
ChangYuHsuan 6:28c4b3c8b43d 364 gx = gx_raw * gRes;
ChangYuHsuan 6:28c4b3c8b43d 365 gy = gy_raw * gRes;
ChangYuHsuan 6:28c4b3c8b43d 366 gz = gz_raw * gRes;
ChangYuHsuan 6:28c4b3c8b43d 367 }
ChangYuHsuan 6:28c4b3c8b43d 368
beanmachine44 0:622e8874902e 369 }
beanmachine44 0:622e8874902e 370
beanmachine44 0:622e8874902e 371 void LSM9DS1::setGyroScale(gyro_scale gScl)
beanmachine44 0:622e8874902e 372 {
beanmachine44 0:622e8874902e 373 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 374 char cmd[2] = {
beanmachine44 0:622e8874902e 375 CTRL_REG1_G,
beanmachine44 0:622e8874902e 376 0
beanmachine44 0:622e8874902e 377 };
beanmachine44 0:622e8874902e 378
beanmachine44 0:622e8874902e 379 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 380 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 381 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 382 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 383
beanmachine44 0:622e8874902e 384 // Then mask out the gyro scale bits:
beanmachine44 0:622e8874902e 385 cmd[1] &= 0xFF^(0x3 << 3);
beanmachine44 0:622e8874902e 386 // Then shift in our new scale bits:
beanmachine44 0:622e8874902e 387 cmd[1] |= gScl << 3;
beanmachine44 0:622e8874902e 388
beanmachine44 0:622e8874902e 389 // Write the gyroscale out to the gyro
beanmachine44 0:622e8874902e 390 i2c.write(xgAddress, cmd, 2);
beanmachine44 0:622e8874902e 391
beanmachine44 0:622e8874902e 392 // We've updated the sensor, but we also need to update our class variables
beanmachine44 0:622e8874902e 393 // First update gScale:
beanmachine44 0:622e8874902e 394 gScale = gScl;
beanmachine44 0:622e8874902e 395 // Then calculate a new gRes, which relies on gScale being set correctly:
beanmachine44 0:622e8874902e 396 calcgRes();
beanmachine44 0:622e8874902e 397 }
beanmachine44 0:622e8874902e 398
beanmachine44 0:622e8874902e 399 void LSM9DS1::setAccelScale(accel_scale aScl)
beanmachine44 0:622e8874902e 400 {
beanmachine44 0:622e8874902e 401 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 402 char cmd[2] = {
beanmachine44 0:622e8874902e 403 CTRL_REG6_XL,
beanmachine44 0:622e8874902e 404 0
beanmachine44 0:622e8874902e 405 };
beanmachine44 0:622e8874902e 406
beanmachine44 0:622e8874902e 407 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 408 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 409 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 410 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 411
beanmachine44 0:622e8874902e 412 // Then mask out the accel scale bits:
beanmachine44 0:622e8874902e 413 cmd[1] &= 0xFF^(0x3 << 3);
beanmachine44 0:622e8874902e 414 // Then shift in our new scale bits:
beanmachine44 0:622e8874902e 415 cmd[1] |= aScl << 3;
beanmachine44 0:622e8874902e 416
beanmachine44 0:622e8874902e 417 // Write the accelscale out to the accel
beanmachine44 0:622e8874902e 418 i2c.write(xgAddress, cmd, 2);
beanmachine44 0:622e8874902e 419
beanmachine44 0:622e8874902e 420 // We've updated the sensor, but we also need to update our class variables
beanmachine44 0:622e8874902e 421 // First update aScale:
beanmachine44 0:622e8874902e 422 aScale = aScl;
beanmachine44 0:622e8874902e 423 // Then calculate a new aRes, which relies on aScale being set correctly:
beanmachine44 0:622e8874902e 424 calcaRes();
beanmachine44 0:622e8874902e 425 }
beanmachine44 0:622e8874902e 426
beanmachine44 0:622e8874902e 427 void LSM9DS1::setMagScale(mag_scale mScl)
beanmachine44 0:622e8874902e 428 {
beanmachine44 0:622e8874902e 429 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 430 char cmd[2] = {
beanmachine44 0:622e8874902e 431 CTRL_REG2_M,
beanmachine44 0:622e8874902e 432 0
beanmachine44 0:622e8874902e 433 };
beanmachine44 0:622e8874902e 434
beanmachine44 0:622e8874902e 435 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 436 i2c.write(mAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 437 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 438 i2c.read(mAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 439
beanmachine44 0:622e8874902e 440 // Then mask out the mag scale bits:
beanmachine44 0:622e8874902e 441 cmd[1] &= 0xFF^(0x3 << 5);
beanmachine44 0:622e8874902e 442 // Then shift in our new scale bits:
beanmachine44 0:622e8874902e 443 cmd[1] |= mScl << 5;
beanmachine44 0:622e8874902e 444
beanmachine44 0:622e8874902e 445 // Write the magscale out to the mag
beanmachine44 0:622e8874902e 446 i2c.write(mAddress, cmd, 2);
beanmachine44 0:622e8874902e 447
beanmachine44 0:622e8874902e 448 // We've updated the sensor, but we also need to update our class variables
beanmachine44 0:622e8874902e 449 // First update mScale:
beanmachine44 0:622e8874902e 450 mScale = mScl;
beanmachine44 0:622e8874902e 451 // Then calculate a new mRes, which relies on mScale being set correctly:
beanmachine44 0:622e8874902e 452 calcmRes();
beanmachine44 0:622e8874902e 453 }
beanmachine44 0:622e8874902e 454
beanmachine44 0:622e8874902e 455 void LSM9DS1::setGyroODR(gyro_odr gRate)
beanmachine44 0:622e8874902e 456 {
5hel2l2y 2:ac3b69ccd3dd 457 char cmd[2];
5hel2l2y 2:ac3b69ccd3dd 458 char cmdLow[2];
5hel2l2y 2:ac3b69ccd3dd 459
5hel2l2y 2:ac3b69ccd3dd 460 if(gRate == G_ODR_15_BW_0 | gRate == G_ODR_60_BW_16 | gRate == G_ODR_119_BW_14 | gRate == G_ODR_119_BW_31) {
5hel2l2y 2:ac3b69ccd3dd 461 cmdLow[0] = CTRL_REG3_G;
5hel2l2y 2:ac3b69ccd3dd 462 cmdLow[1] = 1;
5hel2l2y 2:ac3b69ccd3dd 463
5hel2l2y 2:ac3b69ccd3dd 464 i2c.write(xgAddress, cmdLow, 2);
5hel2l2y 2:ac3b69ccd3dd 465 }
5hel2l2y 2:ac3b69ccd3dd 466
beanmachine44 0:622e8874902e 467 // The start of the addresses we want to read from
5hel2l2y 2:ac3b69ccd3dd 468 cmd[0] = CTRL_REG1_G;
5hel2l2y 2:ac3b69ccd3dd 469 cmd[1] = 0;
beanmachine44 0:622e8874902e 470
beanmachine44 0:622e8874902e 471 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 472 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 473 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 474 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 475
beanmachine44 0:622e8874902e 476 // Then mask out the gyro odr bits:
beanmachine44 0:622e8874902e 477 cmd[1] &= (0x3 << 3);
beanmachine44 0:622e8874902e 478 // Then shift in our new odr bits:
beanmachine44 0:622e8874902e 479 cmd[1] |= gRate;
beanmachine44 0:622e8874902e 480
beanmachine44 0:622e8874902e 481 // Write the gyroodr out to the gyro
beanmachine44 0:622e8874902e 482 i2c.write(xgAddress, cmd, 2);
beanmachine44 0:622e8874902e 483 }
beanmachine44 0:622e8874902e 484
beanmachine44 0:622e8874902e 485 void LSM9DS1::setAccelODR(accel_odr aRate)
beanmachine44 0:622e8874902e 486 {
beanmachine44 0:622e8874902e 487 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 488 char cmd[2] = {
beanmachine44 0:622e8874902e 489 CTRL_REG6_XL,
beanmachine44 0:622e8874902e 490 0
beanmachine44 0:622e8874902e 491 };
beanmachine44 0:622e8874902e 492
beanmachine44 0:622e8874902e 493 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 494 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 495 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 496 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 497
beanmachine44 0:622e8874902e 498 // Then mask out the accel odr bits:
beanmachine44 0:622e8874902e 499 cmd[1] &= 0xFF^(0x7 << 5);
beanmachine44 0:622e8874902e 500 // Then shift in our new odr bits:
beanmachine44 0:622e8874902e 501 cmd[1] |= aRate << 5;
beanmachine44 0:622e8874902e 502
beanmachine44 0:622e8874902e 503 // Write the accelodr out to the accel
beanmachine44 0:622e8874902e 504 i2c.write(xgAddress, cmd, 2);
beanmachine44 0:622e8874902e 505 }
beanmachine44 0:622e8874902e 506
beanmachine44 0:622e8874902e 507 void LSM9DS1::setMagODR(mag_odr mRate)
beanmachine44 0:622e8874902e 508 {
beanmachine44 0:622e8874902e 509 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 510 char cmd[2] = {
beanmachine44 0:622e8874902e 511 CTRL_REG1_M,
beanmachine44 0:622e8874902e 512 0
beanmachine44 0:622e8874902e 513 };
beanmachine44 0:622e8874902e 514
beanmachine44 0:622e8874902e 515 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 516 i2c.write(mAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 517 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 518 i2c.read(mAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 519
beanmachine44 0:622e8874902e 520 // Then mask out the mag odr bits:
beanmachine44 0:622e8874902e 521 cmd[1] &= 0xFF^(0x7 << 2);
beanmachine44 0:622e8874902e 522 // Then shift in our new odr bits:
beanmachine44 0:622e8874902e 523 cmd[1] |= mRate << 2;
beanmachine44 0:622e8874902e 524
beanmachine44 0:622e8874902e 525 // Write the magodr out to the mag
beanmachine44 0:622e8874902e 526 i2c.write(mAddress, cmd, 2);
beanmachine44 0:622e8874902e 527 }
beanmachine44 0:622e8874902e 528
beanmachine44 0:622e8874902e 529 void LSM9DS1::calcgRes()
beanmachine44 0:622e8874902e 530 {
beanmachine44 0:622e8874902e 531 // Possible gyro scales (and their register bit settings) are:
beanmachine44 0:622e8874902e 532 // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
beanmachine44 0:622e8874902e 533 switch (gScale)
beanmachine44 0:622e8874902e 534 {
beanmachine44 0:622e8874902e 535 case G_SCALE_245DPS:
beanmachine44 0:622e8874902e 536 gRes = 245.0 / 32768.0;
beanmachine44 0:622e8874902e 537 break;
beanmachine44 0:622e8874902e 538 case G_SCALE_500DPS:
beanmachine44 0:622e8874902e 539 gRes = 500.0 / 32768.0;
beanmachine44 0:622e8874902e 540 break;
beanmachine44 0:622e8874902e 541 case G_SCALE_2000DPS:
beanmachine44 0:622e8874902e 542 gRes = 2000.0 / 32768.0;
beanmachine44 0:622e8874902e 543 break;
beanmachine44 0:622e8874902e 544 }
beanmachine44 0:622e8874902e 545 }
beanmachine44 0:622e8874902e 546
beanmachine44 0:622e8874902e 547 void LSM9DS1::calcaRes()
beanmachine44 0:622e8874902e 548 {
beanmachine44 0:622e8874902e 549 // Possible accelerometer scales (and their register bit settings) are:
beanmachine44 0:622e8874902e 550 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
beanmachine44 0:622e8874902e 551 switch (aScale)
beanmachine44 0:622e8874902e 552 {
beanmachine44 0:622e8874902e 553 case A_SCALE_2G:
beanmachine44 0:622e8874902e 554 aRes = 2.0 / 32768.0;
beanmachine44 0:622e8874902e 555 break;
beanmachine44 0:622e8874902e 556 case A_SCALE_4G:
beanmachine44 0:622e8874902e 557 aRes = 4.0 / 32768.0;
beanmachine44 0:622e8874902e 558 break;
beanmachine44 0:622e8874902e 559 case A_SCALE_8G:
beanmachine44 0:622e8874902e 560 aRes = 8.0 / 32768.0;
beanmachine44 0:622e8874902e 561 break;
beanmachine44 0:622e8874902e 562 case A_SCALE_16G:
beanmachine44 0:622e8874902e 563 aRes = 16.0 / 32768.0;
beanmachine44 0:622e8874902e 564 break;
beanmachine44 0:622e8874902e 565 }
beanmachine44 0:622e8874902e 566 }
beanmachine44 0:622e8874902e 567
beanmachine44 0:622e8874902e 568 void LSM9DS1::calcmRes()
beanmachine44 0:622e8874902e 569 {
beanmachine44 0:622e8874902e 570 // Possible magnetometer scales (and their register bit settings) are:
beanmachine44 0:622e8874902e 571 // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11).
beanmachine44 0:622e8874902e 572 switch (mScale)
beanmachine44 0:622e8874902e 573 {
beanmachine44 0:622e8874902e 574 case M_SCALE_4GS:
beanmachine44 0:622e8874902e 575 mRes = 4.0 / 32768.0;
beanmachine44 0:622e8874902e 576 break;
beanmachine44 0:622e8874902e 577 case M_SCALE_8GS:
beanmachine44 0:622e8874902e 578 mRes = 8.0 / 32768.0;
beanmachine44 0:622e8874902e 579 break;
beanmachine44 0:622e8874902e 580 case M_SCALE_12GS:
beanmachine44 0:622e8874902e 581 mRes = 12.0 / 32768.0;
beanmachine44 0:622e8874902e 582 break;
beanmachine44 0:622e8874902e 583 case M_SCALE_16GS:
beanmachine44 0:622e8874902e 584 mRes = 16.0 / 32768.0;
beanmachine44 0:622e8874902e 585 break;
beanmachine44 0:622e8874902e 586 }
ChangYuHsuan 6:28c4b3c8b43d 587 }
ChangYuHsuan 6:28c4b3c8b43d 588
ChangYuHsuan 6:28c4b3c8b43d 589
ChangYuHsuan 6:28c4b3c8b43d 590 /*
ChangYuHsuan 6:28c4b3c8b43d 591 void LSM9DS1::enableXgFIFO(bool enable)
ChangYuHsuan 6:28c4b3c8b43d 592 {
ChangYuHsuan 6:28c4b3c8b43d 593 char cmd[2] = {CTRL_REG9, 0};
ChangYuHsuan 6:28c4b3c8b43d 594
ChangYuHsuan 6:28c4b3c8b43d 595 i2c.write(xgAddress, cmd, 1);
ChangYuHsuan 6:28c4b3c8b43d 596 cmd[1] = i2c.read(CTRL_REG9);
ChangYuHsuan 6:28c4b3c8b43d 597
ChangYuHsuan 6:28c4b3c8b43d 598 if (enable) cmd[1] |= (1<<1);
ChangYuHsuan 6:28c4b3c8b43d 599 else cmd[1] &= ~(1<<1);
ChangYuHsuan 6:28c4b3c8b43d 600
ChangYuHsuan 6:28c4b3c8b43d 601 i2c.write(xgAddress, cmd, 2);
ChangYuHsuan 6:28c4b3c8b43d 602 }
ChangYuHsuan 6:28c4b3c8b43d 603
ChangYuHsuan 6:28c4b3c8b43d 604 void LSM9DS1::setXgFIFO(uint8_t fifoMode, uint8_t fifoThs)
ChangYuHsuan 6:28c4b3c8b43d 605 {
ChangYuHsuan 6:28c4b3c8b43d 606 // Limit threshold - 0x1F (31) is the maximum. If more than that was asked
ChangYuHsuan 6:28c4b3c8b43d 607 // limit it to the maximum.
ChangYuHsuan 6:28c4b3c8b43d 608 char cmd[2] = {FIFO_CTRL, 0};
ChangYuHsuan 6:28c4b3c8b43d 609 uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F;
ChangYuHsuan 6:28c4b3c8b43d 610 cmd[1] = ((fifoMode & 0x7) << 5) | (threshold & 0x1F);
ChangYuHsuan 6:28c4b3c8b43d 611 i2c.write(xgAddress, cmd, 2);
ChangYuHsuan 6:28c4b3c8b43d 612 }
ChangYuHsuan 6:28c4b3c8b43d 613 */