Daniel David / MPU6050

Gyroscope and accelerometer sensor based on Korneliusz Jarzebski's lib

Dependents:   weather_station_proj weather_station_project weather_station_proj_v1_2

MPU6050.cpp@0:aef0335c060b, 2018-07-01 (annotated)

Committer:
daniel_davvid
Date:
Sun Jul 01 11:58:49 2018 +0000
Revision:
0:aef0335c060b
Updated so it can work in a weather station project using only the accelerometer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniel_davvid 0:aef0335c060b 1 /*
daniel_davvid 0:aef0335c060b 2 MPU6050.cpp - Class file for the MPU6050 Triple Axis Gyroscope & Accelerometer Arduino Library.
daniel_davvid 0:aef0335c060b 3 Version: 1.0.3
daniel_davvid 0:aef0335c060b 4 (c) 2014-2015 Korneliusz Jarzebski
daniel_davvid 0:aef0335c060b 5 www.jarzebski.pl
daniel_davvid 0:aef0335c060b 6 This program is free software: you can redistribute it and/or modify
daniel_davvid 0:aef0335c060b 7 it under the terms of the version 3 GNU General Public License as
daniel_davvid 0:aef0335c060b 8 published by the Free Software Foundation.
daniel_davvid 0:aef0335c060b 9 This program is distributed in the hope that it will be useful,
daniel_davvid 0:aef0335c060b 10 but WITHOUT ANY WARRANTY; without even the implied warranty of
daniel_davvid 0:aef0335c060b 11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
daniel_davvid 0:aef0335c060b 12 GNU General Public License for more details.
daniel_davvid 0:aef0335c060b 13 You should have received a copy of the GNU General Public License
daniel_davvid 0:aef0335c060b 14 along with this program. If not, see <http://www.gnu.org/licenses/>.
daniel_davvid 0:aef0335c060b 15 */
daniel_davvid 0:aef0335c060b 16
daniel_davvid 0:aef0335c060b 17
daniel_davvid 0:aef0335c060b 18 #include <mbed.h>
daniel_davvid 0:aef0335c060b 19 #include <math.h>
daniel_davvid 0:aef0335c060b 20 #include <new>
daniel_davvid 0:aef0335c060b 21 #include "MPU6050.h"
daniel_davvid 0:aef0335c060b 22
daniel_davvid 0:aef0335c060b 23 MPU6050::MPU6050(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
daniel_davvid 0:aef0335c060b 24 {
daniel_davvid 0:aef0335c060b 25 // Placement new to avoid additional heap memory allocation.
daniel_davvid 0:aef0335c060b 26 new(i2cRaw) I2C(sda, scl);
daniel_davvid 0:aef0335c060b 27 }
daniel_davvid 0:aef0335c060b 28
daniel_davvid 0:aef0335c060b 29 MPU6050::MPU6050(I2C &i2c) : i2c_(i2c)
daniel_davvid 0:aef0335c060b 30 {
daniel_davvid 0:aef0335c060b 31 }
daniel_davvid 0:aef0335c060b 32
daniel_davvid 0:aef0335c060b 33
daniel_davvid 0:aef0335c060b 34 bool MPU6050::begin(mpu6050_dps_t scale, mpu6050_range_t range, int mpua)
daniel_davvid 0:aef0335c060b 35 {
daniel_davvid 0:aef0335c060b 36 // Set Address
daniel_davvid 0:aef0335c060b 37 mpuAddress = mpua;
daniel_davvid 0:aef0335c060b 38 // Reset calibrate values
daniel_davvid 0:aef0335c060b 39 dg.XAxis = 0;
daniel_davvid 0:aef0335c060b 40 dg.YAxis = 0;
daniel_davvid 0:aef0335c060b 41 dg.ZAxis = 0;
daniel_davvid 0:aef0335c060b 42 useCalibrate = false;
daniel_davvid 0:aef0335c060b 43
daniel_davvid 0:aef0335c060b 44 // Reset threshold values
daniel_davvid 0:aef0335c060b 45 tg.XAxis = 0;
daniel_davvid 0:aef0335c060b 46 tg.YAxis = 0;
daniel_davvid 0:aef0335c060b 47 tg.ZAxis = 0;
daniel_davvid 0:aef0335c060b 48 actualThreshold = 0;
daniel_davvid 0:aef0335c060b 49
daniel_davvid 0:aef0335c060b 50 // Check MPU6050 Who Am I Register
daniel_davvid 0:aef0335c060b 51 if (fastRegister8(MPU6050_REG_WHO_AM_I) != 0x68)
daniel_davvid 0:aef0335c060b 52 {
daniel_davvid 0:aef0335c060b 53 return false;
daniel_davvid 0:aef0335c060b 54 }
daniel_davvid 0:aef0335c060b 55
daniel_davvid 0:aef0335c060b 56 // Set Clock Source
daniel_davvid 0:aef0335c060b 57 setClockSource(MPU6050_CLOCK_PLL_XGYRO);
daniel_davvid 0:aef0335c060b 58
daniel_davvid 0:aef0335c060b 59 // Set Scale & Range
daniel_davvid 0:aef0335c060b 60 setScale(scale);
daniel_davvid 0:aef0335c060b 61 setRange(range);
daniel_davvid 0:aef0335c060b 62 //LOW PASS FILTER SETUP
daniel_davvid 0:aef0335c060b 63 setDLPFMode(MPU6050_DLPF_6);
daniel_davvid 0:aef0335c060b 64 // Disable Sleep Mode
daniel_davvid 0:aef0335c060b 65 setSleepEnabled(false);
daniel_davvid 0:aef0335c060b 66
daniel_davvid 0:aef0335c060b 67
daniel_davvid 0:aef0335c060b 68 return true;
daniel_davvid 0:aef0335c060b 69 }
daniel_davvid 0:aef0335c060b 70
daniel_davvid 0:aef0335c060b 71 void MPU6050::setScale(mpu6050_dps_t scale)
daniel_davvid 0:aef0335c060b 72 {
daniel_davvid 0:aef0335c060b 73 uint8_t value;
daniel_davvid 0:aef0335c060b 74
daniel_davvid 0:aef0335c060b 75 switch (scale)
daniel_davvid 0:aef0335c060b 76 {
daniel_davvid 0:aef0335c060b 77 case MPU6050_SCALE_250DPS:
daniel_davvid 0:aef0335c060b 78 dpsPerDigit = .007633f;
daniel_davvid 0:aef0335c060b 79 break;
daniel_davvid 0:aef0335c060b 80 case MPU6050_SCALE_500DPS:
daniel_davvid 0:aef0335c060b 81 dpsPerDigit = .015267f;
daniel_davvid 0:aef0335c060b 82 break;
daniel_davvid 0:aef0335c060b 83 case MPU6050_SCALE_1000DPS:
daniel_davvid 0:aef0335c060b 84 dpsPerDigit = .030487f;
daniel_davvid 0:aef0335c060b 85 break;
daniel_davvid 0:aef0335c060b 86 case MPU6050_SCALE_2000DPS:
daniel_davvid 0:aef0335c060b 87 dpsPerDigit = .060975f;
daniel_davvid 0:aef0335c060b 88 break;
daniel_davvid 0:aef0335c060b 89 default:
daniel_davvid 0:aef0335c060b 90 break;
daniel_davvid 0:aef0335c060b 91 }
daniel_davvid 0:aef0335c060b 92
daniel_davvid 0:aef0335c060b 93 value = readRegister8(MPU6050_REG_GYRO_CONFIG);
daniel_davvid 0:aef0335c060b 94 value &= 0b11100111;
daniel_davvid 0:aef0335c060b 95 value |= (scale << 3);
daniel_davvid 0:aef0335c060b 96 writeRegister8(MPU6050_REG_GYRO_CONFIG, value);
daniel_davvid 0:aef0335c060b 97 }
daniel_davvid 0:aef0335c060b 98
daniel_davvid 0:aef0335c060b 99 mpu6050_dps_t MPU6050::getScale(void)
daniel_davvid 0:aef0335c060b 100 {
daniel_davvid 0:aef0335c060b 101 uint8_t value;
daniel_davvid 0:aef0335c060b 102 value = readRegister8(MPU6050_REG_GYRO_CONFIG);
daniel_davvid 0:aef0335c060b 103 value &= 0b00011000;
daniel_davvid 0:aef0335c060b 104 value >>= 3;
daniel_davvid 0:aef0335c060b 105 return (mpu6050_dps_t)value;
daniel_davvid 0:aef0335c060b 106 }
daniel_davvid 0:aef0335c060b 107
daniel_davvid 0:aef0335c060b 108 void MPU6050::setRange(mpu6050_range_t range)
daniel_davvid 0:aef0335c060b 109 {
daniel_davvid 0:aef0335c060b 110 uint8_t value;
daniel_davvid 0:aef0335c060b 111
daniel_davvid 0:aef0335c060b 112 switch (range)
daniel_davvid 0:aef0335c060b 113 {
daniel_davvid 0:aef0335c060b 114 case MPU6050_RANGE_2G:
daniel_davvid 0:aef0335c060b 115 rangePerDigit = .000061f;
daniel_davvid 0:aef0335c060b 116 break;
daniel_davvid 0:aef0335c060b 117 case MPU6050_RANGE_4G:
daniel_davvid 0:aef0335c060b 118 rangePerDigit = .000122f;
daniel_davvid 0:aef0335c060b 119 break;
daniel_davvid 0:aef0335c060b 120 case MPU6050_RANGE_8G:
daniel_davvid 0:aef0335c060b 121 rangePerDigit = .000244f;
daniel_davvid 0:aef0335c060b 122 break;
daniel_davvid 0:aef0335c060b 123 case MPU6050_RANGE_16G:
daniel_davvid 0:aef0335c060b 124 rangePerDigit = .0004882f;
daniel_davvid 0:aef0335c060b 125 break;
daniel_davvid 0:aef0335c060b 126 default:
daniel_davvid 0:aef0335c060b 127 break;
daniel_davvid 0:aef0335c060b 128 }
daniel_davvid 0:aef0335c060b 129
daniel_davvid 0:aef0335c060b 130 value = readRegister8(MPU6050_REG_ACCEL_CONFIG);
daniel_davvid 0:aef0335c060b 131 value &= 0b11100111;
daniel_davvid 0:aef0335c060b 132 value |= (range << 3);
daniel_davvid 0:aef0335c060b 133 writeRegister8(MPU6050_REG_ACCEL_CONFIG, value);
daniel_davvid 0:aef0335c060b 134 }
daniel_davvid 0:aef0335c060b 135
daniel_davvid 0:aef0335c060b 136 mpu6050_range_t MPU6050::getRange(void)
daniel_davvid 0:aef0335c060b 137 {
daniel_davvid 0:aef0335c060b 138 uint8_t value;
daniel_davvid 0:aef0335c060b 139 value = readRegister8(MPU6050_REG_ACCEL_CONFIG);
daniel_davvid 0:aef0335c060b 140 value &= 0b00011000;
daniel_davvid 0:aef0335c060b 141 value >>= 3;
daniel_davvid 0:aef0335c060b 142 return (mpu6050_range_t)value;
daniel_davvid 0:aef0335c060b 143 }
daniel_davvid 0:aef0335c060b 144
daniel_davvid 0:aef0335c060b 145 void MPU6050::setDHPFMode(mpu6050_dhpf_t dhpf)
daniel_davvid 0:aef0335c060b 146 {
daniel_davvid 0:aef0335c060b 147 uint8_t value;
daniel_davvid 0:aef0335c060b 148 value = readRegister8(MPU6050_REG_ACCEL_CONFIG);
daniel_davvid 0:aef0335c060b 149 value &= 0b11111000;
daniel_davvid 0:aef0335c060b 150 value |= dhpf;
daniel_davvid 0:aef0335c060b 151 writeRegister8(MPU6050_REG_ACCEL_CONFIG, value);
daniel_davvid 0:aef0335c060b 152 }
daniel_davvid 0:aef0335c060b 153
daniel_davvid 0:aef0335c060b 154 void MPU6050::setDLPFMode(mpu6050_dlpf_t dlpf)
daniel_davvid 0:aef0335c060b 155 {
daniel_davvid 0:aef0335c060b 156 uint8_t value;
daniel_davvid 0:aef0335c060b 157 value = readRegister8(MPU6050_REG_CONFIG);
daniel_davvid 0:aef0335c060b 158 value &= 0b11111000;
daniel_davvid 0:aef0335c060b 159 value |= dlpf;
daniel_davvid 0:aef0335c060b 160 writeRegister8(MPU6050_REG_CONFIG, value);
daniel_davvid 0:aef0335c060b 161 }
daniel_davvid 0:aef0335c060b 162
daniel_davvid 0:aef0335c060b 163 void MPU6050::setClockSource(mpu6050_clockSource_t source)
daniel_davvid 0:aef0335c060b 164 {
daniel_davvid 0:aef0335c060b 165 uint8_t value;
daniel_davvid 0:aef0335c060b 166 value = readRegister8(MPU6050_REG_PWR_MGMT_1);
daniel_davvid 0:aef0335c060b 167 value &= 0b11111000;
daniel_davvid 0:aef0335c060b 168 value |= source;
daniel_davvid 0:aef0335c060b 169 writeRegister8(MPU6050_REG_PWR_MGMT_1, value);
daniel_davvid 0:aef0335c060b 170 }
daniel_davvid 0:aef0335c060b 171
daniel_davvid 0:aef0335c060b 172 mpu6050_clockSource_t MPU6050::getClockSource(void)
daniel_davvid 0:aef0335c060b 173 {
daniel_davvid 0:aef0335c060b 174 uint8_t value;
daniel_davvid 0:aef0335c060b 175 value = readRegister8(MPU6050_REG_PWR_MGMT_1);
daniel_davvid 0:aef0335c060b 176 value &= 0b00000111;
daniel_davvid 0:aef0335c060b 177 return (mpu6050_clockSource_t)value;
daniel_davvid 0:aef0335c060b 178 }
daniel_davvid 0:aef0335c060b 179
daniel_davvid 0:aef0335c060b 180 bool MPU6050::getSleepEnabled(void)
daniel_davvid 0:aef0335c060b 181 {
daniel_davvid 0:aef0335c060b 182 return readRegisterBit(MPU6050_REG_PWR_MGMT_1, 6);
daniel_davvid 0:aef0335c060b 183 }
daniel_davvid 0:aef0335c060b 184
daniel_davvid 0:aef0335c060b 185 void MPU6050::setSleepEnabled(bool state)
daniel_davvid 0:aef0335c060b 186 {
daniel_davvid 0:aef0335c060b 187 writeRegisterBit(MPU6050_REG_PWR_MGMT_1, 6, state);
daniel_davvid 0:aef0335c060b 188 }
daniel_davvid 0:aef0335c060b 189
daniel_davvid 0:aef0335c060b 190 bool MPU6050::getIntZeroMotionEnabled(void)
daniel_davvid 0:aef0335c060b 191 {
daniel_davvid 0:aef0335c060b 192 return readRegisterBit(MPU6050_REG_INT_ENABLE, 5);
daniel_davvid 0:aef0335c060b 193 }
daniel_davvid 0:aef0335c060b 194
daniel_davvid 0:aef0335c060b 195 void MPU6050::setIntZeroMotionEnabled(bool state)
daniel_davvid 0:aef0335c060b 196 {
daniel_davvid 0:aef0335c060b 197 writeRegisterBit(MPU6050_REG_INT_ENABLE, 5, state);
daniel_davvid 0:aef0335c060b 198 }
daniel_davvid 0:aef0335c060b 199
daniel_davvid 0:aef0335c060b 200 bool MPU6050::getIntMotionEnabled(void)
daniel_davvid 0:aef0335c060b 201 {
daniel_davvid 0:aef0335c060b 202 return readRegisterBit(MPU6050_REG_INT_ENABLE, 6);
daniel_davvid 0:aef0335c060b 203 }
daniel_davvid 0:aef0335c060b 204
daniel_davvid 0:aef0335c060b 205 void MPU6050::setIntMotionEnabled(bool state)
daniel_davvid 0:aef0335c060b 206 {
daniel_davvid 0:aef0335c060b 207 writeRegisterBit(MPU6050_REG_INT_ENABLE, 6, state);
daniel_davvid 0:aef0335c060b 208 }
daniel_davvid 0:aef0335c060b 209
daniel_davvid 0:aef0335c060b 210 bool MPU6050::getIntFreeFallEnabled(void)
daniel_davvid 0:aef0335c060b 211 {
daniel_davvid 0:aef0335c060b 212 return readRegisterBit(MPU6050_REG_INT_ENABLE, 7);
daniel_davvid 0:aef0335c060b 213 }
daniel_davvid 0:aef0335c060b 214
daniel_davvid 0:aef0335c060b 215 void MPU6050::setIntFreeFallEnabled(bool state)
daniel_davvid 0:aef0335c060b 216 {
daniel_davvid 0:aef0335c060b 217 writeRegisterBit(MPU6050_REG_INT_ENABLE, 7, state);
daniel_davvid 0:aef0335c060b 218 }
daniel_davvid 0:aef0335c060b 219
daniel_davvid 0:aef0335c060b 220 uint8_t MPU6050::getMotionDetectionThreshold(void)
daniel_davvid 0:aef0335c060b 221 {
daniel_davvid 0:aef0335c060b 222 return readRegister8(MPU6050_REG_MOT_THRESHOLD);
daniel_davvid 0:aef0335c060b 223 }
daniel_davvid 0:aef0335c060b 224
daniel_davvid 0:aef0335c060b 225 void MPU6050::setMotionDetectionThreshold(uint8_t threshold)
daniel_davvid 0:aef0335c060b 226 {
daniel_davvid 0:aef0335c060b 227 writeRegister8(MPU6050_REG_MOT_THRESHOLD, threshold);
daniel_davvid 0:aef0335c060b 228 }
daniel_davvid 0:aef0335c060b 229
daniel_davvid 0:aef0335c060b 230 uint8_t MPU6050::getMotionDetectionDuration(void)
daniel_davvid 0:aef0335c060b 231 {
daniel_davvid 0:aef0335c060b 232 return readRegister8(MPU6050_REG_MOT_DURATION);
daniel_davvid 0:aef0335c060b 233 }
daniel_davvid 0:aef0335c060b 234
daniel_davvid 0:aef0335c060b 235 void MPU6050::setMotionDetectionDuration(uint8_t duration)
daniel_davvid 0:aef0335c060b 236 {
daniel_davvid 0:aef0335c060b 237 writeRegister8(MPU6050_REG_MOT_DURATION, duration);
daniel_davvid 0:aef0335c060b 238 }
daniel_davvid 0:aef0335c060b 239
daniel_davvid 0:aef0335c060b 240 uint8_t MPU6050::getZeroMotionDetectionThreshold(void)
daniel_davvid 0:aef0335c060b 241 {
daniel_davvid 0:aef0335c060b 242 return readRegister8(MPU6050_REG_ZMOT_THRESHOLD);
daniel_davvid 0:aef0335c060b 243 }
daniel_davvid 0:aef0335c060b 244
daniel_davvid 0:aef0335c060b 245 void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold)
daniel_davvid 0:aef0335c060b 246 {
daniel_davvid 0:aef0335c060b 247 writeRegister8(MPU6050_REG_ZMOT_THRESHOLD, threshold);
daniel_davvid 0:aef0335c060b 248 }
daniel_davvid 0:aef0335c060b 249
daniel_davvid 0:aef0335c060b 250 uint8_t MPU6050::getZeroMotionDetectionDuration(void)
daniel_davvid 0:aef0335c060b 251 {
daniel_davvid 0:aef0335c060b 252 return readRegister8(MPU6050_REG_ZMOT_DURATION);
daniel_davvid 0:aef0335c060b 253 }
daniel_davvid 0:aef0335c060b 254
daniel_davvid 0:aef0335c060b 255 void MPU6050::setZeroMotionDetectionDuration(uint8_t duration)
daniel_davvid 0:aef0335c060b 256 {
daniel_davvid 0:aef0335c060b 257 writeRegister8(MPU6050_REG_ZMOT_DURATION, duration);
daniel_davvid 0:aef0335c060b 258 }
daniel_davvid 0:aef0335c060b 259
daniel_davvid 0:aef0335c060b 260 uint8_t MPU6050::getFreeFallDetectionThreshold(void)
daniel_davvid 0:aef0335c060b 261 {
daniel_davvid 0:aef0335c060b 262 return readRegister8(MPU6050_REG_FF_THRESHOLD);
daniel_davvid 0:aef0335c060b 263 }
daniel_davvid 0:aef0335c060b 264
daniel_davvid 0:aef0335c060b 265 void MPU6050::setFreeFallDetectionThreshold(uint8_t threshold)
daniel_davvid 0:aef0335c060b 266 {
daniel_davvid 0:aef0335c060b 267 writeRegister8(MPU6050_REG_FF_THRESHOLD, threshold);
daniel_davvid 0:aef0335c060b 268 }
daniel_davvid 0:aef0335c060b 269
daniel_davvid 0:aef0335c060b 270 uint8_t MPU6050::getFreeFallDetectionDuration(void)
daniel_davvid 0:aef0335c060b 271 {
daniel_davvid 0:aef0335c060b 272 return readRegister8(MPU6050_REG_FF_DURATION);
daniel_davvid 0:aef0335c060b 273 }
daniel_davvid 0:aef0335c060b 274
daniel_davvid 0:aef0335c060b 275 void MPU6050::setFreeFallDetectionDuration(uint8_t duration)
daniel_davvid 0:aef0335c060b 276 {
daniel_davvid 0:aef0335c060b 277 writeRegister8(MPU6050_REG_FF_DURATION, duration);
daniel_davvid 0:aef0335c060b 278 }
daniel_davvid 0:aef0335c060b 279
daniel_davvid 0:aef0335c060b 280 bool MPU6050::getI2CMasterModeEnabled(void)
daniel_davvid 0:aef0335c060b 281 {
daniel_davvid 0:aef0335c060b 282 return readRegisterBit(MPU6050_REG_USER_CTRL, 5);
daniel_davvid 0:aef0335c060b 283 }
daniel_davvid 0:aef0335c060b 284
daniel_davvid 0:aef0335c060b 285 void MPU6050::setI2CMasterModeEnabled(bool state)
daniel_davvid 0:aef0335c060b 286 {
daniel_davvid 0:aef0335c060b 287 writeRegisterBit(MPU6050_REG_USER_CTRL, 5, state);
daniel_davvid 0:aef0335c060b 288 }
daniel_davvid 0:aef0335c060b 289
daniel_davvid 0:aef0335c060b 290 void MPU6050::setI2CBypassEnabled(bool state)
daniel_davvid 0:aef0335c060b 291 {
daniel_davvid 0:aef0335c060b 292 return writeRegisterBit(MPU6050_REG_INT_PIN_CFG, 1, state);
daniel_davvid 0:aef0335c060b 293 }
daniel_davvid 0:aef0335c060b 294
daniel_davvid 0:aef0335c060b 295 bool MPU6050::getI2CBypassEnabled(void)
daniel_davvid 0:aef0335c060b 296 {
daniel_davvid 0:aef0335c060b 297 return readRegisterBit(MPU6050_REG_INT_PIN_CFG, 1);
daniel_davvid 0:aef0335c060b 298 }
daniel_davvid 0:aef0335c060b 299
daniel_davvid 0:aef0335c060b 300 void MPU6050::setAccelPowerOnDelay(mpu6050_onDelay_t delay)
daniel_davvid 0:aef0335c060b 301 {
daniel_davvid 0:aef0335c060b 302 uint8_t value;
daniel_davvid 0:aef0335c060b 303 value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL);
daniel_davvid 0:aef0335c060b 304 value &= 0b11001111;
daniel_davvid 0:aef0335c060b 305 value |= (delay << 4);
daniel_davvid 0:aef0335c060b 306 writeRegister8(MPU6050_REG_MOT_DETECT_CTRL, value);
daniel_davvid 0:aef0335c060b 307 }
daniel_davvid 0:aef0335c060b 308
daniel_davvid 0:aef0335c060b 309 mpu6050_onDelay_t MPU6050::getAccelPowerOnDelay(void)
daniel_davvid 0:aef0335c060b 310 {
daniel_davvid 0:aef0335c060b 311 uint8_t value;
daniel_davvid 0:aef0335c060b 312 value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL);
daniel_davvid 0:aef0335c060b 313 value &= 0b00110000;
daniel_davvid 0:aef0335c060b 314 return (mpu6050_onDelay_t)(value >> 4);
daniel_davvid 0:aef0335c060b 315 }
daniel_davvid 0:aef0335c060b 316
daniel_davvid 0:aef0335c060b 317 uint8_t MPU6050::getIntStatus(void)
daniel_davvid 0:aef0335c060b 318 {
daniel_davvid 0:aef0335c060b 319 return readRegister8(MPU6050_REG_INT_STATUS);
daniel_davvid 0:aef0335c060b 320 }
daniel_davvid 0:aef0335c060b 321
daniel_davvid 0:aef0335c060b 322 Activites MPU6050::readActivites(void)
daniel_davvid 0:aef0335c060b 323 {
daniel_davvid 0:aef0335c060b 324 uint8_t data = readRegister8(MPU6050_REG_INT_STATUS);
daniel_davvid 0:aef0335c060b 325
daniel_davvid 0:aef0335c060b 326 a.isOverflow = ((data >> 4) & 1);
daniel_davvid 0:aef0335c060b 327 a.isFreeFall = ((data >> 7) & 1);
daniel_davvid 0:aef0335c060b 328 a.isInactivity = ((data >> 5) & 1);
daniel_davvid 0:aef0335c060b 329 a.isActivity = ((data >> 6) & 1);
daniel_davvid 0:aef0335c060b 330 a.isDataReady = ((data >> 0) & 1);
daniel_davvid 0:aef0335c060b 331
daniel_davvid 0:aef0335c060b 332 data = readRegister8(MPU6050_REG_MOT_DETECT_STATUS);
daniel_davvid 0:aef0335c060b 333
daniel_davvid 0:aef0335c060b 334 a.isNegActivityOnX = ((data >> 7) & 1);
daniel_davvid 0:aef0335c060b 335 a.isPosActivityOnX = ((data >> 6) & 1);
daniel_davvid 0:aef0335c060b 336
daniel_davvid 0:aef0335c060b 337 a.isNegActivityOnY = ((data >> 5) & 1);
daniel_davvid 0:aef0335c060b 338 a.isPosActivityOnY = ((data >> 4) & 1);
daniel_davvid 0:aef0335c060b 339
daniel_davvid 0:aef0335c060b 340 a.isNegActivityOnZ = ((data >> 3) & 1);
daniel_davvid 0:aef0335c060b 341 a.isPosActivityOnZ = ((data >> 2) & 1);
daniel_davvid 0:aef0335c060b 342
daniel_davvid 0:aef0335c060b 343 return a;
daniel_davvid 0:aef0335c060b 344 }
daniel_davvid 0:aef0335c060b 345
daniel_davvid 0:aef0335c060b 346 Vector MPU6050::readRawAccel(void)
daniel_davvid 0:aef0335c060b 347 {
daniel_davvid 0:aef0335c060b 348 char data = MPU6050_REG_ACCEL_XOUT_H;
daniel_davvid 0:aef0335c060b 349 i2c_.write(mpuAddress,&data, 1, true);
daniel_davvid 0:aef0335c060b 350
daniel_davvid 0:aef0335c060b 351
daniel_davvid 0:aef0335c060b 352 uint8_t value[6];
daniel_davvid 0:aef0335c060b 353 i2c_.read(mpuAddress, (char *)&value, 6);
daniel_davvid 0:aef0335c060b 354
daniel_davvid 0:aef0335c060b 355 uint8_t xha = value[0];
daniel_davvid 0:aef0335c060b 356 uint8_t xla = value[1];
daniel_davvid 0:aef0335c060b 357 uint8_t yha = value[2];
daniel_davvid 0:aef0335c060b 358 uint8_t yla = value[3];
daniel_davvid 0:aef0335c060b 359 uint8_t zha = value[4];
daniel_davvid 0:aef0335c060b 360 uint8_t zla = value[5];
daniel_davvid 0:aef0335c060b 361
daniel_davvid 0:aef0335c060b 362
daniel_davvid 0:aef0335c060b 363 ra.XAxis = xha << 8 | xla;
daniel_davvid 0:aef0335c060b 364 ra.YAxis = yha << 8 | yla;
daniel_davvid 0:aef0335c060b 365 ra.ZAxis = zha << 8 | zla;
daniel_davvid 0:aef0335c060b 366
daniel_davvid 0:aef0335c060b 367 return ra;
daniel_davvid 0:aef0335c060b 368 }
daniel_davvid 0:aef0335c060b 369
daniel_davvid 0:aef0335c060b 370 Vector MPU6050::readNormalizeAccel(void)
daniel_davvid 0:aef0335c060b 371 {
daniel_davvid 0:aef0335c060b 372 readRawAccel();
daniel_davvid 0:aef0335c060b 373
daniel_davvid 0:aef0335c060b 374 na.XAxis = ra.XAxis * rangePerDigit * 9.80665f;
daniel_davvid 0:aef0335c060b 375 na.YAxis = ra.YAxis * rangePerDigit * 9.80665f;
daniel_davvid 0:aef0335c060b 376 na.ZAxis = ra.ZAxis * rangePerDigit * 9.80665f;
daniel_davvid 0:aef0335c060b 377
daniel_davvid 0:aef0335c060b 378 return na;
daniel_davvid 0:aef0335c060b 379 }
daniel_davvid 0:aef0335c060b 380
daniel_davvid 0:aef0335c060b 381 Vector MPU6050::readScaledAccel(void)
daniel_davvid 0:aef0335c060b 382 {
daniel_davvid 0:aef0335c060b 383 readRawAccel();
daniel_davvid 0:aef0335c060b 384
daniel_davvid 0:aef0335c060b 385 na.XAxis = ra.XAxis * rangePerDigit;
daniel_davvid 0:aef0335c060b 386 na.YAxis = ra.YAxis * rangePerDigit;
daniel_davvid 0:aef0335c060b 387 na.ZAxis = ra.ZAxis * rangePerDigit;
daniel_davvid 0:aef0335c060b 388
daniel_davvid 0:aef0335c060b 389 return na;
daniel_davvid 0:aef0335c060b 390 }
daniel_davvid 0:aef0335c060b 391
daniel_davvid 0:aef0335c060b 392
daniel_davvid 0:aef0335c060b 393 Vector MPU6050::readRawGyro(void)
daniel_davvid 0:aef0335c060b 394 {
daniel_davvid 0:aef0335c060b 395 char data = MPU6050_REG_GYRO_XOUT_H;
daniel_davvid 0:aef0335c060b 396 i2c_.write(mpuAddress,&data, 1, true);
daniel_davvid 0:aef0335c060b 397
daniel_davvid 0:aef0335c060b 398 uint8_t value[6];
daniel_davvid 0:aef0335c060b 399
daniel_davvid 0:aef0335c060b 400 i2c_.read(mpuAddress, (char *)&value, 6);
daniel_davvid 0:aef0335c060b 401
daniel_davvid 0:aef0335c060b 402 uint8_t xha = value[0];
daniel_davvid 0:aef0335c060b 403 uint8_t xla = value[1];
daniel_davvid 0:aef0335c060b 404 uint8_t yha = value[2];
daniel_davvid 0:aef0335c060b 405 uint8_t yla = value[3];
daniel_davvid 0:aef0335c060b 406 uint8_t zha = value[4];
daniel_davvid 0:aef0335c060b 407 uint8_t zla = value[5];
daniel_davvid 0:aef0335c060b 408
daniel_davvid 0:aef0335c060b 409
daniel_davvid 0:aef0335c060b 410 rg.XAxis = xha << 8 | xla;
daniel_davvid 0:aef0335c060b 411 rg.YAxis = yha << 8 | yla;
daniel_davvid 0:aef0335c060b 412 rg.ZAxis = zha << 8 | zla;
daniel_davvid 0:aef0335c060b 413
daniel_davvid 0:aef0335c060b 414 return rg;
daniel_davvid 0:aef0335c060b 415 }
daniel_davvid 0:aef0335c060b 416
daniel_davvid 0:aef0335c060b 417 Vector MPU6050::readNormalizeGyro(void)
daniel_davvid 0:aef0335c060b 418 {
daniel_davvid 0:aef0335c060b 419 readRawGyro();
daniel_davvid 0:aef0335c060b 420
daniel_davvid 0:aef0335c060b 421 if (useCalibrate)
daniel_davvid 0:aef0335c060b 422 {
daniel_davvid 0:aef0335c060b 423 ng.XAxis = (rg.XAxis - dg.XAxis) * dpsPerDigit;
daniel_davvid 0:aef0335c060b 424 ng.YAxis = (rg.YAxis - dg.YAxis) * dpsPerDigit;
daniel_davvid 0:aef0335c060b 425 ng.ZAxis = (rg.ZAxis - dg.ZAxis) * dpsPerDigit;
daniel_davvid 0:aef0335c060b 426 } else
daniel_davvid 0:aef0335c060b 427 {
daniel_davvid 0:aef0335c060b 428 ng.XAxis = rg.XAxis * dpsPerDigit;
daniel_davvid 0:aef0335c060b 429 ng.YAxis = rg.YAxis * dpsPerDigit;
daniel_davvid 0:aef0335c060b 430 ng.ZAxis = rg.ZAxis * dpsPerDigit;
daniel_davvid 0:aef0335c060b 431 }
daniel_davvid 0:aef0335c060b 432
daniel_davvid 0:aef0335c060b 433 if (actualThreshold)
daniel_davvid 0:aef0335c060b 434 {
daniel_davvid 0:aef0335c060b 435 if (abs(ng.XAxis) < tg.XAxis) ng.XAxis = 0;
daniel_davvid 0:aef0335c060b 436 if (abs(ng.YAxis) < tg.YAxis) ng.YAxis = 0;
daniel_davvid 0:aef0335c060b 437 if (abs(ng.ZAxis) < tg.ZAxis) ng.ZAxis = 0;
daniel_davvid 0:aef0335c060b 438 }
daniel_davvid 0:aef0335c060b 439
daniel_davvid 0:aef0335c060b 440 return ng;
daniel_davvid 0:aef0335c060b 441 }
daniel_davvid 0:aef0335c060b 442
daniel_davvid 0:aef0335c060b 443 float MPU6050::readTemperature(void)
daniel_davvid 0:aef0335c060b 444 {
daniel_davvid 0:aef0335c060b 445 int16_t T;
daniel_davvid 0:aef0335c060b 446 T = readRegister16(MPU6050_REG_TEMP_OUT_H);
daniel_davvid 0:aef0335c060b 447 return (float)T/340 + 36.53f;
daniel_davvid 0:aef0335c060b 448 }
daniel_davvid 0:aef0335c060b 449
daniel_davvid 0:aef0335c060b 450 int16_t MPU6050::getGyroOffsetX(void)
daniel_davvid 0:aef0335c060b 451 {
daniel_davvid 0:aef0335c060b 452 return readRegister16(MPU6050_REG_GYRO_XOFFS_H);
daniel_davvid 0:aef0335c060b 453 }
daniel_davvid 0:aef0335c060b 454
daniel_davvid 0:aef0335c060b 455 int16_t MPU6050::getGyroOffsetY(void)
daniel_davvid 0:aef0335c060b 456 {
daniel_davvid 0:aef0335c060b 457 return readRegister16(MPU6050_REG_GYRO_YOFFS_H);
daniel_davvid 0:aef0335c060b 458 }
daniel_davvid 0:aef0335c060b 459
daniel_davvid 0:aef0335c060b 460 int16_t MPU6050::getGyroOffsetZ(void)
daniel_davvid 0:aef0335c060b 461 {
daniel_davvid 0:aef0335c060b 462 return readRegister16(MPU6050_REG_GYRO_ZOFFS_H);
daniel_davvid 0:aef0335c060b 463 }
daniel_davvid 0:aef0335c060b 464
daniel_davvid 0:aef0335c060b 465 void MPU6050::setGyroOffsetX(int16_t offset)
daniel_davvid 0:aef0335c060b 466 {
daniel_davvid 0:aef0335c060b 467 writeRegister16(MPU6050_REG_GYRO_XOFFS_H, offset);
daniel_davvid 0:aef0335c060b 468 }
daniel_davvid 0:aef0335c060b 469
daniel_davvid 0:aef0335c060b 470 void MPU6050::setGyroOffsetY(int16_t offset)
daniel_davvid 0:aef0335c060b 471 {
daniel_davvid 0:aef0335c060b 472 writeRegister16(MPU6050_REG_GYRO_YOFFS_H, offset);
daniel_davvid 0:aef0335c060b 473 }
daniel_davvid 0:aef0335c060b 474
daniel_davvid 0:aef0335c060b 475 void MPU6050::setGyroOffsetZ(int16_t offset)
daniel_davvid 0:aef0335c060b 476 {
daniel_davvid 0:aef0335c060b 477 writeRegister16(MPU6050_REG_GYRO_ZOFFS_H, offset);
daniel_davvid 0:aef0335c060b 478 }
daniel_davvid 0:aef0335c060b 479
daniel_davvid 0:aef0335c060b 480 int16_t MPU6050::getAccelOffsetX(void)
daniel_davvid 0:aef0335c060b 481 {
daniel_davvid 0:aef0335c060b 482 return readRegister16(MPU6050_REG_ACCEL_XOFFS_H);
daniel_davvid 0:aef0335c060b 483 }
daniel_davvid 0:aef0335c060b 484
daniel_davvid 0:aef0335c060b 485 int16_t MPU6050::getAccelOffsetY(void)
daniel_davvid 0:aef0335c060b 486 {
daniel_davvid 0:aef0335c060b 487 return readRegister16(MPU6050_REG_ACCEL_YOFFS_H);
daniel_davvid 0:aef0335c060b 488 }
daniel_davvid 0:aef0335c060b 489
daniel_davvid 0:aef0335c060b 490 int16_t MPU6050::getAccelOffsetZ(void)
daniel_davvid 0:aef0335c060b 491 {
daniel_davvid 0:aef0335c060b 492 return readRegister16(MPU6050_REG_ACCEL_ZOFFS_H);
daniel_davvid 0:aef0335c060b 493 }
daniel_davvid 0:aef0335c060b 494
daniel_davvid 0:aef0335c060b 495 void MPU6050::setAccelOffsetX(int16_t offset)
daniel_davvid 0:aef0335c060b 496 {
daniel_davvid 0:aef0335c060b 497 writeRegister16(MPU6050_REG_ACCEL_XOFFS_H, offset);
daniel_davvid 0:aef0335c060b 498 }
daniel_davvid 0:aef0335c060b 499
daniel_davvid 0:aef0335c060b 500 void MPU6050::setAccelOffsetY(int16_t offset)
daniel_davvid 0:aef0335c060b 501 {
daniel_davvid 0:aef0335c060b 502 writeRegister16(MPU6050_REG_ACCEL_YOFFS_H, offset);
daniel_davvid 0:aef0335c060b 503 }
daniel_davvid 0:aef0335c060b 504
daniel_davvid 0:aef0335c060b 505 void MPU6050::setAccelOffsetZ(int16_t offset)
daniel_davvid 0:aef0335c060b 506 {
daniel_davvid 0:aef0335c060b 507 writeRegister16(MPU6050_REG_ACCEL_ZOFFS_H, offset);
daniel_davvid 0:aef0335c060b 508 }
daniel_davvid 0:aef0335c060b 509
daniel_davvid 0:aef0335c060b 510 // Calibrate algorithm
daniel_davvid 0:aef0335c060b 511 void MPU6050::calibrateGyro(uint8_t samples)
daniel_davvid 0:aef0335c060b 512 {
daniel_davvid 0:aef0335c060b 513 // Set calibrate
daniel_davvid 0:aef0335c060b 514 useCalibrate = true;
daniel_davvid 0:aef0335c060b 515
daniel_davvid 0:aef0335c060b 516 // Reset values
daniel_davvid 0:aef0335c060b 517 float sumX = 0;
daniel_davvid 0:aef0335c060b 518 float sumY = 0;
daniel_davvid 0:aef0335c060b 519 float sumZ = 0;
daniel_davvid 0:aef0335c060b 520 float sigmaX = 0;
daniel_davvid 0:aef0335c060b 521 float sigmaY = 0;
daniel_davvid 0:aef0335c060b 522 float sigmaZ = 0;
daniel_davvid 0:aef0335c060b 523
daniel_davvid 0:aef0335c060b 524 // Read n-samples
daniel_davvid 0:aef0335c060b 525 for (uint8_t i = 0; i < samples; ++i)
daniel_davvid 0:aef0335c060b 526 {
daniel_davvid 0:aef0335c060b 527 readRawGyro();
daniel_davvid 0:aef0335c060b 528 sumX += rg.XAxis;
daniel_davvid 0:aef0335c060b 529 sumY += rg.YAxis;
daniel_davvid 0:aef0335c060b 530 sumZ += rg.ZAxis;
daniel_davvid 0:aef0335c060b 531
daniel_davvid 0:aef0335c060b 532 sigmaX += rg.XAxis * rg.XAxis;
daniel_davvid 0:aef0335c060b 533 sigmaY += rg.YAxis * rg.YAxis;
daniel_davvid 0:aef0335c060b 534 sigmaZ += rg.ZAxis * rg.ZAxis;
daniel_davvid 0:aef0335c060b 535
daniel_davvid 0:aef0335c060b 536 wait(0.005f);
daniel_davvid 0:aef0335c060b 537 }
daniel_davvid 0:aef0335c060b 538
daniel_davvid 0:aef0335c060b 539 // Calculate delta vectors
daniel_davvid 0:aef0335c060b 540 dg.XAxis = sumX / samples;
daniel_davvid 0:aef0335c060b 541 dg.YAxis = sumY / samples;
daniel_davvid 0:aef0335c060b 542 dg.ZAxis = sumZ / samples;
daniel_davvid 0:aef0335c060b 543
daniel_davvid 0:aef0335c060b 544 // Calculate threshold vectors
daniel_davvid 0:aef0335c060b 545 th.XAxis = sqrt((sigmaX / 50) - (dg.XAxis * dg.XAxis));
daniel_davvid 0:aef0335c060b 546 th.YAxis = sqrt((sigmaY / 50) - (dg.YAxis * dg.YAxis));
daniel_davvid 0:aef0335c060b 547 th.ZAxis = sqrt((sigmaZ / 50) - (dg.ZAxis * dg.ZAxis));
daniel_davvid 0:aef0335c060b 548
daniel_davvid 0:aef0335c060b 549 // If already set threshold, recalculate threshold vectors
daniel_davvid 0:aef0335c060b 550 if (actualThreshold > 0)
daniel_davvid 0:aef0335c060b 551 {
daniel_davvid 0:aef0335c060b 552 setThreshold(actualThreshold);
daniel_davvid 0:aef0335c060b 553 }
daniel_davvid 0:aef0335c060b 554 }
daniel_davvid 0:aef0335c060b 555
daniel_davvid 0:aef0335c060b 556 // Get current threshold value
daniel_davvid 0:aef0335c060b 557 uint8_t MPU6050::getThreshold(void)
daniel_davvid 0:aef0335c060b 558 {
daniel_davvid 0:aef0335c060b 559 return actualThreshold;
daniel_davvid 0:aef0335c060b 560 }
daniel_davvid 0:aef0335c060b 561
daniel_davvid 0:aef0335c060b 562 // Set treshold value
daniel_davvid 0:aef0335c060b 563 void MPU6050::setThreshold(uint8_t multiple)
daniel_davvid 0:aef0335c060b 564 {
daniel_davvid 0:aef0335c060b 565 if (multiple > 0)
daniel_davvid 0:aef0335c060b 566 {
daniel_davvid 0:aef0335c060b 567 // If not calibrated, need calibrate
daniel_davvid 0:aef0335c060b 568 if (!useCalibrate)
daniel_davvid 0:aef0335c060b 569 {
daniel_davvid 0:aef0335c060b 570 calibrateGyro();
daniel_davvid 0:aef0335c060b 571 }
daniel_davvid 0:aef0335c060b 572
daniel_davvid 0:aef0335c060b 573 // Calculate threshold vectors
daniel_davvid 0:aef0335c060b 574 tg.XAxis = th.XAxis * multiple;
daniel_davvid 0:aef0335c060b 575 tg.YAxis = th.YAxis * multiple;
daniel_davvid 0:aef0335c060b 576 tg.ZAxis = th.ZAxis * multiple;
daniel_davvid 0:aef0335c060b 577 } else
daniel_davvid 0:aef0335c060b 578 {
daniel_davvid 0:aef0335c060b 579 // No threshold
daniel_davvid 0:aef0335c060b 580 tg.XAxis = 0;
daniel_davvid 0:aef0335c060b 581 tg.YAxis = 0;
daniel_davvid 0:aef0335c060b 582 tg.ZAxis = 0;
daniel_davvid 0:aef0335c060b 583 }
daniel_davvid 0:aef0335c060b 584
daniel_davvid 0:aef0335c060b 585 // Remember old threshold value
daniel_davvid 0:aef0335c060b 586 actualThreshold = multiple;
daniel_davvid 0:aef0335c060b 587 }
daniel_davvid 0:aef0335c060b 588
daniel_davvid 0:aef0335c060b 589 // Fast read 8-bit from register
daniel_davvid 0:aef0335c060b 590 uint8_t MPU6050::fastRegister8(uint8_t reg, bool repeated)
daniel_davvid 0:aef0335c060b 591 {
daniel_davvid 0:aef0335c060b 592 uint8_t value;
daniel_davvid 0:aef0335c060b 593 i2c_.write(mpuAddress,(char *)&reg, 1, true);
daniel_davvid 0:aef0335c060b 594 i2c_.read(mpuAddress,(char *)&value, 1, repeated);
daniel_davvid 0:aef0335c060b 595
daniel_davvid 0:aef0335c060b 596 return value;
daniel_davvid 0:aef0335c060b 597 }
daniel_davvid 0:aef0335c060b 598
daniel_davvid 0:aef0335c060b 599 // Read 8-bit from register
daniel_davvid 0:aef0335c060b 600 uint8_t MPU6050::readRegister8(uint8_t reg, bool repeated)
daniel_davvid 0:aef0335c060b 601 {
daniel_davvid 0:aef0335c060b 602 uint8_t value;
daniel_davvid 0:aef0335c060b 603 i2c_.write(mpuAddress,(char *)&reg, 1, true);
daniel_davvid 0:aef0335c060b 604 i2c_.read(mpuAddress,(char *)&value, 1, repeated);
daniel_davvid 0:aef0335c060b 605
daniel_davvid 0:aef0335c060b 606 return value;
daniel_davvid 0:aef0335c060b 607 }
daniel_davvid 0:aef0335c060b 608
daniel_davvid 0:aef0335c060b 609 // Write 8-bit to register
daniel_davvid 0:aef0335c060b 610 void MPU6050::writeRegister8(uint8_t reg, uint8_t value, bool repeated)
daniel_davvid 0:aef0335c060b 611 {
daniel_davvid 0:aef0335c060b 612 uint8_t cmd[2];
daniel_davvid 0:aef0335c060b 613 cmd[0] = reg;
daniel_davvid 0:aef0335c060b 614 cmd[1] = value;
daniel_davvid 0:aef0335c060b 615 i2c_.write(mpuAddress,(char *)&cmd, 2, repeated);
daniel_davvid 0:aef0335c060b 616 }
daniel_davvid 0:aef0335c060b 617
daniel_davvid 0:aef0335c060b 618 int16_t MPU6050::readRegister16(uint8_t reg, bool repeated)
daniel_davvid 0:aef0335c060b 619 {
daniel_davvid 0:aef0335c060b 620 int16_t value;
daniel_davvid 0:aef0335c060b 621 i2c_.write(mpuAddress, (char *)&reg, 1, true);
daniel_davvid 0:aef0335c060b 622 uint8_t readings[2];
daniel_davvid 0:aef0335c060b 623 i2c_.read(mpuAddress, (char *)&readings, 2, repeated);
daniel_davvid 0:aef0335c060b 624 uint8_t vha = readings[0];
daniel_davvid 0:aef0335c060b 625 uint8_t vla = readings[1];
daniel_davvid 0:aef0335c060b 626
daniel_davvid 0:aef0335c060b 627 value = vha << 8 | vla;
daniel_davvid 0:aef0335c060b 628
daniel_davvid 0:aef0335c060b 629 return value;
daniel_davvid 0:aef0335c060b 630 }
daniel_davvid 0:aef0335c060b 631
daniel_davvid 0:aef0335c060b 632 void MPU6050::writeRegister16(uint8_t reg, int16_t value, bool repeated)
daniel_davvid 0:aef0335c060b 633 {
daniel_davvid 0:aef0335c060b 634 i2c_.write(mpuAddress, (char *)&reg, 1, true);
daniel_davvid 0:aef0335c060b 635 i2c_.write(mpuAddress, (char *)&value, 2, repeated);
daniel_davvid 0:aef0335c060b 636 }
daniel_davvid 0:aef0335c060b 637
daniel_davvid 0:aef0335c060b 638 // Read register bit
daniel_davvid 0:aef0335c060b 639 bool MPU6050::readRegisterBit(uint8_t reg, uint8_t pos)
daniel_davvid 0:aef0335c060b 640 {
daniel_davvid 0:aef0335c060b 641 uint8_t value;
daniel_davvid 0:aef0335c060b 642 value = readRegister8(reg);
daniel_davvid 0:aef0335c060b 643 return ((value >> pos) & 1);
daniel_davvid 0:aef0335c060b 644 }
daniel_davvid 0:aef0335c060b 645
daniel_davvid 0:aef0335c060b 646 // Write register bit
daniel_davvid 0:aef0335c060b 647 void MPU6050::writeRegisterBit(uint8_t reg, uint8_t pos, bool state)
daniel_davvid 0:aef0335c060b 648 {
daniel_davvid 0:aef0335c060b 649 uint8_t value;
daniel_davvid 0:aef0335c060b 650 value = readRegister8(reg);
daniel_davvid 0:aef0335c060b 651
daniel_davvid 0:aef0335c060b 652 if (state)
daniel_davvid 0:aef0335c060b 653 {
daniel_davvid 0:aef0335c060b 654 value |= (1 << pos);
daniel_davvid 0:aef0335c060b 655 } else
daniel_davvid 0:aef0335c060b 656 {
daniel_davvid 0:aef0335c060b 657 value &= ~(1 << pos);
daniel_davvid 0:aef0335c060b 658 }
daniel_davvid 0:aef0335c060b 659
daniel_davvid 0:aef0335c060b 660 writeRegister8(reg, value);
daniel_davvid 0:aef0335c060b 661 }