Chen Wei Ting / Mbed 2 deprecated LSM9DS1_project_4_torque

read encoder and Dynamixel angle

Dependencies:   MX28 PID mbed

LSM9DS1.cpp@0:2dbbe7d28fab, 2018-08-04 (annotated)

Committer:
JJting
Date:
Sat Aug 04 05:55:50 2018 +0000
Revision:
0:2dbbe7d28fab
can read encoder and Dynamixel angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JJting 0:2dbbe7d28fab 1 #include "LSM9DS1.h"
JJting 0:2dbbe7d28fab 2
JJting 0:2dbbe7d28fab 3 LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr) : i2c(sda, scl)
JJting 0:2dbbe7d28fab 4 {
JJting 0:2dbbe7d28fab 5 // xgAddress and mAddress will store the 7-bit I2C address, if using I2C.
JJting 0:2dbbe7d28fab 6 xgAddress = xgAddr;
JJting 0:2dbbe7d28fab 7 mAddress = mAddr;
JJting 0:2dbbe7d28fab 8 }
JJting 0:2dbbe7d28fab 9
JJting 0:2dbbe7d28fab 10 uint16_t LSM9DS1::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
JJting 0:2dbbe7d28fab 11 gyro_odr gODR, accel_odr aODR, mag_odr mODR)
JJting 0:2dbbe7d28fab 12 {
JJting 0:2dbbe7d28fab 13 // Store the given scales in class variables. These scale variables
JJting 0:2dbbe7d28fab 14 // are used throughout to calculate the actual g's, DPS,and Gs's.
JJting 0:2dbbe7d28fab 15 gScale = gScl;
JJting 0:2dbbe7d28fab 16 aScale = aScl;
JJting 0:2dbbe7d28fab 17 mScale = mScl;
JJting 0:2dbbe7d28fab 18
JJting 0:2dbbe7d28fab 19 // Once we have the scale values, we can calculate the resolution
JJting 0:2dbbe7d28fab 20 // of each sensor. That's what these functions are for. One for each sensor
JJting 0:2dbbe7d28fab 21 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
JJting 0:2dbbe7d28fab 22 calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
JJting 0:2dbbe7d28fab 23 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
JJting 0:2dbbe7d28fab 24
JJting 0:2dbbe7d28fab 25
JJting 0:2dbbe7d28fab 26 // To verify communication, we can read from the WHO_AM_I register of
JJting 0:2dbbe7d28fab 27 // each device. Store those in a variable so we can return them.
JJting 0:2dbbe7d28fab 28 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 29 char cmd[2] = {
JJting 0:2dbbe7d28fab 30 WHO_AM_I_XG,
JJting 0:2dbbe7d28fab 31 0
JJting 0:2dbbe7d28fab 32 };
JJting 0:2dbbe7d28fab 33
JJting 0:2dbbe7d28fab 34 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 35 i2c.write(xgAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 36 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 37 i2c.read(xgAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 38 uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I
JJting 0:2dbbe7d28fab 39
JJting 0:2dbbe7d28fab 40 // Reset to the address of the mag who am i
JJting 0:2dbbe7d28fab 41 cmd[1] = WHO_AM_I_M;
JJting 0:2dbbe7d28fab 42 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 43 i2c.write(mAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 44 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 45 i2c.read(mAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 46 uint8_t mTest = cmd[1]; // Read the mag WHO_AM_I
JJting 0:2dbbe7d28fab 47
JJting 0:2dbbe7d28fab 48 // Gyro initialization stuff:
JJting 0:2dbbe7d28fab 49 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
JJting 0:2dbbe7d28fab 50 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
JJting 0:2dbbe7d28fab 51 setGyroScale(gScale); // Set the gyro range
JJting 0:2dbbe7d28fab 52
JJting 0:2dbbe7d28fab 53 // Accelerometer initialization stuff:
JJting 0:2dbbe7d28fab 54 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
JJting 0:2dbbe7d28fab 55 setAccelODR(aODR); // Set the accel data rate.
JJting 0:2dbbe7d28fab 56 setAccelScale(aScale); // Set the accel range.
JJting 0:2dbbe7d28fab 57
JJting 0:2dbbe7d28fab 58 // Magnetometer initialization stuff:
JJting 0:2dbbe7d28fab 59 initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
JJting 0:2dbbe7d28fab 60 setMagODR(mODR); // Set the magnetometer output data rate.
JJting 0:2dbbe7d28fab 61 setMagScale(mScale); // Set the magnetometer's range.
JJting 0:2dbbe7d28fab 62
JJting 0:2dbbe7d28fab 63 // Once everything is initialized, return the WHO_AM_I registers we read:
JJting 0:2dbbe7d28fab 64 return (xgTest << 8) | mTest;
JJting 0:2dbbe7d28fab 65 }
JJting 0:2dbbe7d28fab 66
JJting 0:2dbbe7d28fab 67 void LSM9DS1::initGyro()
JJting 0:2dbbe7d28fab 68 {
JJting 0:2dbbe7d28fab 69 char cmd[4] = {
JJting 0:2dbbe7d28fab 70 CTRL_REG1_G,
JJting 0:2dbbe7d28fab 71 gScale | G_ODR_119_BW_14,
JJting 0:2dbbe7d28fab 72 0, // Default data out and int out
JJting 0:2dbbe7d28fab 73 0 // Default power mode and high pass settings
JJting 0:2dbbe7d28fab 74 };
JJting 0:2dbbe7d28fab 75
JJting 0:2dbbe7d28fab 76 // Write the data to the gyro control registers
JJting 0:2dbbe7d28fab 77 i2c.write(xgAddress, cmd, 4);
JJting 0:2dbbe7d28fab 78 }
JJting 0:2dbbe7d28fab 79
JJting 0:2dbbe7d28fab 80 void LSM9DS1::initAccel()
JJting 0:2dbbe7d28fab 81 {
JJting 0:2dbbe7d28fab 82 char cmd[4] = {
JJting 0:2dbbe7d28fab 83 CTRL_REG5_XL,
JJting 0:2dbbe7d28fab 84 0x38, // Enable all axis and don't decimate data in out Registers
JJting 0:2dbbe7d28fab 85 (A_ODR_119 << 5) | (aScale << 3) | (A_BW_AUTO_SCALE), // 119 Hz ODR, set scale, and auto BW
JJting 0:2dbbe7d28fab 86 0 // Default resolution mode and filtering settings
JJting 0:2dbbe7d28fab 87 };
JJting 0:2dbbe7d28fab 88
JJting 0:2dbbe7d28fab 89 // Write the data to the accel control registers
JJting 0:2dbbe7d28fab 90 i2c.write(xgAddress, cmd, 4);
JJting 0:2dbbe7d28fab 91 }
JJting 0:2dbbe7d28fab 92
JJting 0:2dbbe7d28fab 93 void LSM9DS1::initMag()
JJting 0:2dbbe7d28fab 94 {
JJting 0:2dbbe7d28fab 95 char cmd[4] = {
JJting 0:2dbbe7d28fab 96 CTRL_REG1_M,
JJting 0:2dbbe7d28fab 97 0x10, // Default data rate, xy axes mode, and temp comp
JJting 0:2dbbe7d28fab 98 mScale << 5, // Set mag scale
JJting 0:2dbbe7d28fab 99 0 // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
JJting 0:2dbbe7d28fab 100 };
JJting 0:2dbbe7d28fab 101
JJting 0:2dbbe7d28fab 102 // Write the data to the mag control registers
JJting 0:2dbbe7d28fab 103 i2c.write(mAddress, cmd, 4);
JJting 0:2dbbe7d28fab 104 }
JJting 0:2dbbe7d28fab 105
JJting 0:2dbbe7d28fab 106 void LSM9DS1::readAccel()
JJting 0:2dbbe7d28fab 107 {
JJting 0:2dbbe7d28fab 108 // The data we are going to read from the accel
JJting 0:2dbbe7d28fab 109 char data[6];
JJting 0:2dbbe7d28fab 110
JJting 0:2dbbe7d28fab 111 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 112 char subAddress = OUT_X_L_XL;
JJting 0:2dbbe7d28fab 113
JJting 0:2dbbe7d28fab 114 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 115 i2c.write(xgAddress, &subAddress, 1, true);
JJting 0:2dbbe7d28fab 116 // Read in all 8 bit registers containing the axes data
JJting 0:2dbbe7d28fab 117 i2c.read(xgAddress, data, 6);
JJting 0:2dbbe7d28fab 118
JJting 0:2dbbe7d28fab 119 // Reassemble the data and convert to g
JJting 0:2dbbe7d28fab 120 ax_raw = data[0] | (data[1] << 8);
JJting 0:2dbbe7d28fab 121 ay_raw = data[2] | (data[3] << 8);
JJting 0:2dbbe7d28fab 122 az_raw = data[4] | (data[5] << 8);
JJting 0:2dbbe7d28fab 123 ax = ax_raw * aRes;
JJting 0:2dbbe7d28fab 124 ay = ay_raw * aRes;
JJting 0:2dbbe7d28fab 125 az = az_raw * aRes;
JJting 0:2dbbe7d28fab 126 }
JJting 0:2dbbe7d28fab 127
JJting 0:2dbbe7d28fab 128 void LSM9DS1::readMag()
JJting 0:2dbbe7d28fab 129 {
JJting 0:2dbbe7d28fab 130 // The data we are going to read from the mag
JJting 0:2dbbe7d28fab 131 char data[6];
JJting 0:2dbbe7d28fab 132
JJting 0:2dbbe7d28fab 133 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 134 char subAddress = OUT_X_L_M;
JJting 0:2dbbe7d28fab 135
JJting 0:2dbbe7d28fab 136 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 137 i2c.write(mAddress, &subAddress, 1, true);
JJting 0:2dbbe7d28fab 138 // Read in all 8 bit registers containing the axes data
JJting 0:2dbbe7d28fab 139 i2c.read(mAddress, data, 6);
JJting 0:2dbbe7d28fab 140
JJting 0:2dbbe7d28fab 141 // Reassemble the data and convert to degrees
JJting 0:2dbbe7d28fab 142 mx_raw = data[0] | (data[1] << 8);
JJting 0:2dbbe7d28fab 143 my_raw = data[2] | (data[3] << 8);
JJting 0:2dbbe7d28fab 144 mz_raw = data[4] | (data[5] << 8);
JJting 0:2dbbe7d28fab 145 mx = mx_raw * mRes;
JJting 0:2dbbe7d28fab 146 my = my_raw * mRes;
JJting 0:2dbbe7d28fab 147 mz = mz_raw * mRes;
JJting 0:2dbbe7d28fab 148 }
JJting 0:2dbbe7d28fab 149
JJting 0:2dbbe7d28fab 150 void LSM9DS1::readTemp()
JJting 0:2dbbe7d28fab 151 {
JJting 0:2dbbe7d28fab 152 // The data we are going to read from the temp
JJting 0:2dbbe7d28fab 153 char data[2];
JJting 0:2dbbe7d28fab 154
JJting 0:2dbbe7d28fab 155 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 156 char subAddress = OUT_TEMP_L;
JJting 0:2dbbe7d28fab 157
JJting 0:2dbbe7d28fab 158 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 159 i2c.write(xgAddress, &subAddress, 1, true);
JJting 0:2dbbe7d28fab 160 // Read in all 8 bit registers containing the axes data
JJting 0:2dbbe7d28fab 161 i2c.read(xgAddress, data, 2);
JJting 0:2dbbe7d28fab 162
JJting 0:2dbbe7d28fab 163 // Temperature is a 12-bit signed integer
JJting 0:2dbbe7d28fab 164 temperature_raw = data[0] | (data[1] << 8);
JJting 0:2dbbe7d28fab 165
JJting 0:2dbbe7d28fab 166 temperature_c = (float)temperature_raw / 8.0 + 25;
JJting 0:2dbbe7d28fab 167 temperature_f = temperature_c * 1.8 + 32;
JJting 0:2dbbe7d28fab 168 }
JJting 0:2dbbe7d28fab 169
JJting 0:2dbbe7d28fab 170
JJting 0:2dbbe7d28fab 171 void LSM9DS1::readGyro()
JJting 0:2dbbe7d28fab 172 {
JJting 0:2dbbe7d28fab 173 // The data we are going to read from the gyro
JJting 0:2dbbe7d28fab 174 char data[6];
JJting 0:2dbbe7d28fab 175
JJting 0:2dbbe7d28fab 176 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 177 char subAddress = OUT_X_L_G;
JJting 0:2dbbe7d28fab 178
JJting 0:2dbbe7d28fab 179 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 180 i2c.write(xgAddress, &subAddress, 1, true);
JJting 0:2dbbe7d28fab 181 // Read in all 8 bit registers containing the axes data
JJting 0:2dbbe7d28fab 182 i2c.read(xgAddress, data, 6);
JJting 0:2dbbe7d28fab 183
JJting 0:2dbbe7d28fab 184 // Reassemble the data and convert to degrees/sec
JJting 0:2dbbe7d28fab 185 gx_raw = data[0] | (data[1] << 8);
JJting 0:2dbbe7d28fab 186 gy_raw = data[2] | (data[3] << 8);
JJting 0:2dbbe7d28fab 187 gz_raw = data[4] | (data[5] << 8);
JJting 0:2dbbe7d28fab 188 gx = gx_raw * gRes;
JJting 0:2dbbe7d28fab 189 gy = gy_raw * gRes;
JJting 0:2dbbe7d28fab 190 gz = gz_raw * gRes;
JJting 0:2dbbe7d28fab 191 }
JJting 0:2dbbe7d28fab 192
JJting 0:2dbbe7d28fab 193 void LSM9DS1::setGyroScale(gyro_scale gScl)
JJting 0:2dbbe7d28fab 194 {
JJting 0:2dbbe7d28fab 195 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 196 char cmd[2] = {
JJting 0:2dbbe7d28fab 197 CTRL_REG1_G,
JJting 0:2dbbe7d28fab 198 0
JJting 0:2dbbe7d28fab 199 };
JJting 0:2dbbe7d28fab 200
JJting 0:2dbbe7d28fab 201 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 202 i2c.write(xgAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 203 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 204 i2c.read(xgAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 205
JJting 0:2dbbe7d28fab 206 // Then mask out the gyro scale bits:
JJting 0:2dbbe7d28fab 207 cmd[1] &= 0xFF^(0x3 << 3);
JJting 0:2dbbe7d28fab 208 // Then shift in our new scale bits:
JJting 0:2dbbe7d28fab 209 cmd[1] |= gScl << 3;
JJting 0:2dbbe7d28fab 210
JJting 0:2dbbe7d28fab 211 // Write the gyroscale out to the gyro
JJting 0:2dbbe7d28fab 212 i2c.write(xgAddress, cmd, 2);
JJting 0:2dbbe7d28fab 213
JJting 0:2dbbe7d28fab 214 // We've updated the sensor, but we also need to update our class variables
JJting 0:2dbbe7d28fab 215 // First update gScale:
JJting 0:2dbbe7d28fab 216 gScale = gScl;
JJting 0:2dbbe7d28fab 217 // Then calculate a new gRes, which relies on gScale being set correctly:
JJting 0:2dbbe7d28fab 218 calcgRes();
JJting 0:2dbbe7d28fab 219 }
JJting 0:2dbbe7d28fab 220
JJting 0:2dbbe7d28fab 221 void LSM9DS1::setAccelScale(accel_scale aScl)
JJting 0:2dbbe7d28fab 222 {
JJting 0:2dbbe7d28fab 223 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 224 char cmd[2] = {
JJting 0:2dbbe7d28fab 225 CTRL_REG6_XL,
JJting 0:2dbbe7d28fab 226 0
JJting 0:2dbbe7d28fab 227 };
JJting 0:2dbbe7d28fab 228
JJting 0:2dbbe7d28fab 229 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 230 i2c.write(xgAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 231 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 232 i2c.read(xgAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 233
JJting 0:2dbbe7d28fab 234 // Then mask out the accel scale bits:
JJting 0:2dbbe7d28fab 235 cmd[1] &= 0xFF^(0x3 << 3);
JJting 0:2dbbe7d28fab 236 // Then shift in our new scale bits:
JJting 0:2dbbe7d28fab 237 cmd[1] |= aScl << 3;
JJting 0:2dbbe7d28fab 238
JJting 0:2dbbe7d28fab 239 // Write the accelscale out to the accel
JJting 0:2dbbe7d28fab 240 i2c.write(xgAddress, cmd, 2);
JJting 0:2dbbe7d28fab 241
JJting 0:2dbbe7d28fab 242 // We've updated the sensor, but we also need to update our class variables
JJting 0:2dbbe7d28fab 243 // First update aScale:
JJting 0:2dbbe7d28fab 244 aScale = aScl;
JJting 0:2dbbe7d28fab 245 // Then calculate a new aRes, which relies on aScale being set correctly:
JJting 0:2dbbe7d28fab 246 calcaRes();
JJting 0:2dbbe7d28fab 247 }
JJting 0:2dbbe7d28fab 248
JJting 0:2dbbe7d28fab 249 void LSM9DS1::setMagScale(mag_scale mScl)
JJting 0:2dbbe7d28fab 250 {
JJting 0:2dbbe7d28fab 251 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 252 char cmd[2] = {
JJting 0:2dbbe7d28fab 253 CTRL_REG2_M,
JJting 0:2dbbe7d28fab 254 0
JJting 0:2dbbe7d28fab 255 };
JJting 0:2dbbe7d28fab 256
JJting 0:2dbbe7d28fab 257 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 258 i2c.write(mAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 259 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 260 i2c.read(mAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 261
JJting 0:2dbbe7d28fab 262 // Then mask out the mag scale bits:
JJting 0:2dbbe7d28fab 263 cmd[1] &= 0xFF^(0x3 << 5);
JJting 0:2dbbe7d28fab 264 // Then shift in our new scale bits:
JJting 0:2dbbe7d28fab 265 cmd[1] |= mScl << 5;
JJting 0:2dbbe7d28fab 266
JJting 0:2dbbe7d28fab 267 // Write the magscale out to the mag
JJting 0:2dbbe7d28fab 268 i2c.write(mAddress, cmd, 2);
JJting 0:2dbbe7d28fab 269
JJting 0:2dbbe7d28fab 270 // We've updated the sensor, but we also need to update our class variables
JJting 0:2dbbe7d28fab 271 // First update mScale:
JJting 0:2dbbe7d28fab 272 mScale = mScl;
JJting 0:2dbbe7d28fab 273 // Then calculate a new mRes, which relies on mScale being set correctly:
JJting 0:2dbbe7d28fab 274 calcmRes();
JJting 0:2dbbe7d28fab 275 }
JJting 0:2dbbe7d28fab 276
JJting 0:2dbbe7d28fab 277 void LSM9DS1::setGyroODR(gyro_odr gRate)
JJting 0:2dbbe7d28fab 278 {
JJting 0:2dbbe7d28fab 279 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 280 char cmd[2] = {
JJting 0:2dbbe7d28fab 281 CTRL_REG1_G,
JJting 0:2dbbe7d28fab 282 0
JJting 0:2dbbe7d28fab 283 };
JJting 0:2dbbe7d28fab 284
JJting 0:2dbbe7d28fab 285 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 286 i2c.write(xgAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 287 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 288 i2c.read(xgAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 289
JJting 0:2dbbe7d28fab 290 // Then mask out the gyro odr bits:
JJting 0:2dbbe7d28fab 291 cmd[1] &= (0x3 << 3);
JJting 0:2dbbe7d28fab 292 // Then shift in our new odr bits:
JJting 0:2dbbe7d28fab 293 cmd[1] |= gRate;
JJting 0:2dbbe7d28fab 294
JJting 0:2dbbe7d28fab 295 // Write the gyroodr out to the gyro
JJting 0:2dbbe7d28fab 296 i2c.write(xgAddress, cmd, 2);
JJting 0:2dbbe7d28fab 297 }
JJting 0:2dbbe7d28fab 298
JJting 0:2dbbe7d28fab 299 void LSM9DS1::setAccelODR(accel_odr aRate)
JJting 0:2dbbe7d28fab 300 {
JJting 0:2dbbe7d28fab 301 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 302 char cmd[2] = {
JJting 0:2dbbe7d28fab 303 CTRL_REG6_XL,
JJting 0:2dbbe7d28fab 304 0
JJting 0:2dbbe7d28fab 305 };
JJting 0:2dbbe7d28fab 306
JJting 0:2dbbe7d28fab 307 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 308 i2c.write(xgAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 309 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 310 i2c.read(xgAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 311
JJting 0:2dbbe7d28fab 312 // Then mask out the accel odr bits:
JJting 0:2dbbe7d28fab 313 cmd[1] &= 0xFF^(0x7 << 5);
JJting 0:2dbbe7d28fab 314 // Then shift in our new odr bits:
JJting 0:2dbbe7d28fab 315 cmd[1] |= aRate << 5;
JJting 0:2dbbe7d28fab 316
JJting 0:2dbbe7d28fab 317 // Write the accelodr out to the accel
JJting 0:2dbbe7d28fab 318 i2c.write(xgAddress, cmd, 2);
JJting 0:2dbbe7d28fab 319 }
JJting 0:2dbbe7d28fab 320
JJting 0:2dbbe7d28fab 321 void LSM9DS1::setMagODR(mag_odr mRate)
JJting 0:2dbbe7d28fab 322 {
JJting 0:2dbbe7d28fab 323 // The start of the addresses we want to read from
JJting 0:2dbbe7d28fab 324 char cmd[2] = {
JJting 0:2dbbe7d28fab 325 CTRL_REG1_M,
JJting 0:2dbbe7d28fab 326 0
JJting 0:2dbbe7d28fab 327 };
JJting 0:2dbbe7d28fab 328
JJting 0:2dbbe7d28fab 329 // Write the address we are going to read from and don't end the transaction
JJting 0:2dbbe7d28fab 330 i2c.write(mAddress, cmd, 1, true);
JJting 0:2dbbe7d28fab 331 // Read in all the 8 bits of data
JJting 0:2dbbe7d28fab 332 i2c.read(mAddress, cmd+1, 1);
JJting 0:2dbbe7d28fab 333
JJting 0:2dbbe7d28fab 334 // Then mask out the mag odr bits:
JJting 0:2dbbe7d28fab 335 cmd[1] &= 0xFF^(0x7 << 2);
JJting 0:2dbbe7d28fab 336 // Then shift in our new odr bits:
JJting 0:2dbbe7d28fab 337 cmd[1] |= mRate << 2;
JJting 0:2dbbe7d28fab 338
JJting 0:2dbbe7d28fab 339 // Write the magodr out to the mag
JJting 0:2dbbe7d28fab 340 i2c.write(mAddress, cmd, 2);
JJting 0:2dbbe7d28fab 341 }
JJting 0:2dbbe7d28fab 342
JJting 0:2dbbe7d28fab 343 void LSM9DS1::calcgRes()
JJting 0:2dbbe7d28fab 344 {
JJting 0:2dbbe7d28fab 345 // Possible gyro scales (and their register bit settings) are:
JJting 0:2dbbe7d28fab 346 // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
JJting 0:2dbbe7d28fab 347 switch (gScale)
JJting 0:2dbbe7d28fab 348 {
JJting 0:2dbbe7d28fab 349 case G_SCALE_245DPS:
JJting 0:2dbbe7d28fab 350 gRes = 245.0 / 32768.0;
JJting 0:2dbbe7d28fab 351 break;
JJting 0:2dbbe7d28fab 352 case G_SCALE_500DPS:
JJting 0:2dbbe7d28fab 353 gRes = 500.0 / 32768.0;
JJting 0:2dbbe7d28fab 354 break;
JJting 0:2dbbe7d28fab 355 case G_SCALE_2000DPS:
JJting 0:2dbbe7d28fab 356 gRes = 2000.0 / 32768.0;
JJting 0:2dbbe7d28fab 357 break;
JJting 0:2dbbe7d28fab 358 }
JJting 0:2dbbe7d28fab 359 }
JJting 0:2dbbe7d28fab 360
JJting 0:2dbbe7d28fab 361 void LSM9DS1::calcaRes()
JJting 0:2dbbe7d28fab 362 {
JJting 0:2dbbe7d28fab 363 // Possible accelerometer scales (and their register bit settings) are:
JJting 0:2dbbe7d28fab 364 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
JJting 0:2dbbe7d28fab 365 switch (aScale)
JJting 0:2dbbe7d28fab 366 {
JJting 0:2dbbe7d28fab 367 case A_SCALE_2G:
JJting 0:2dbbe7d28fab 368 aRes = 2.0 / 32768.0;
JJting 0:2dbbe7d28fab 369 break;
JJting 0:2dbbe7d28fab 370 case A_SCALE_4G:
JJting 0:2dbbe7d28fab 371 aRes = 4.0 / 32768.0;
JJting 0:2dbbe7d28fab 372 break;
JJting 0:2dbbe7d28fab 373 case A_SCALE_8G:
JJting 0:2dbbe7d28fab 374 aRes = 8.0 / 32768.0;
JJting 0:2dbbe7d28fab 375 break;
JJting 0:2dbbe7d28fab 376 case A_SCALE_16G:
JJting 0:2dbbe7d28fab 377 aRes = 16.0 / 32768.0;
JJting 0:2dbbe7d28fab 378 break;
JJting 0:2dbbe7d28fab 379 }
JJting 0:2dbbe7d28fab 380 }
JJting 0:2dbbe7d28fab 381
JJting 0:2dbbe7d28fab 382 void LSM9DS1::calcmRes()
JJting 0:2dbbe7d28fab 383 {
JJting 0:2dbbe7d28fab 384 // Possible magnetometer scales (and their register bit settings) are:
JJting 0:2dbbe7d28fab 385 // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11).
JJting 0:2dbbe7d28fab 386 switch (mScale)
JJting 0:2dbbe7d28fab 387 {
JJting 0:2dbbe7d28fab 388 case M_SCALE_4GS:
JJting 0:2dbbe7d28fab 389 mRes = 4.0 / 32768.0;
JJting 0:2dbbe7d28fab 390 break;
JJting 0:2dbbe7d28fab 391 case M_SCALE_8GS:
JJting 0:2dbbe7d28fab 392 mRes = 8.0 / 32768.0;
JJting 0:2dbbe7d28fab 393 break;
JJting 0:2dbbe7d28fab 394 case M_SCALE_12GS:
JJting 0:2dbbe7d28fab 395 mRes = 12.0 / 32768.0;
JJting 0:2dbbe7d28fab 396 break;
JJting 0:2dbbe7d28fab 397 case M_SCALE_16GS:
JJting 0:2dbbe7d28fab 398 mRes = 16.0 / 32768.0;
JJting 0:2dbbe7d28fab 399 break;
JJting 0:2dbbe7d28fab 400 }
JJting 0:2dbbe7d28fab 401 }