Chen Wei Ting / Mbed 2 deprecated zerotorque_final

zero torque and encoder

Dependencies:   MX28 PID mbed

Fork of LSM9DS1_project_5_zerotorque by Chen Wei Ting

LSM9DS1.cpp@5:131450b16ce3, 2018-08-31 (annotated)

Committer:
JJting
Date:
Fri Aug 31 01:41:04 2018 +0000
Revision:
5:131450b16ce3
Parent:
0:c23e915f255b 
????OK P=1500 D=40

Who changed what in which revision?

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