Allen Wild / LSM9DS0

I2C Library for the LSM9DS0 IMU

Dependents:   4180_LSM9DS0_lab HW2_P2 HW2_P3 HW2_P4 ... more

LSM9DS0.cpp@0:3a1dce39106c, 2015-01-26 (annotated)

Committer:
aswild
Date:
Mon Jan 26 06:34:53 2015 +0000
Revision:
0:3a1dce39106c
Child:
2:5556e6fb99f5
make LSM9DS0 a library

Who changed what in which revision?

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