Felix Rüdiger
An abstraction layer for the I2C communication with a MPU6050 accelerometer/gyroscope
Dependents: BLE_Nano_MPU6050Service
Embed:
(wiki syntax)
Show/hide line numbers
MPU6050.cpp
00001 #include "MPU6050.h" 00002 00003 bool MPU6050::read(Register reg, uint8_t *data, size_t length, float timeout_secs) 00004 { 00005 float t = 0.0f; 00006 00007 uint8_t tmp = reg; 00008 00009 while ((i2c.write(baseAddress & BYTE(11111110), (char*)&tmp, 1, true) != 0) || (i2c.read(baseAddress | BYTE(00000001), (char*)data, length) != 0)) 00010 if ((t += retryDelay_secs) > timeout_secs) 00011 return false; 00012 else 00013 wait(retryDelay_secs); 00014 00015 return true; 00016 } 00017 00018 inline bool MPU6050::read(Register reg, uint8_t *data, float timeout_secs) 00019 { 00020 return this->read(reg, data, 1, timeout_secs); 00021 } 00022 00023 bool MPU6050::write(Register reg, uint8_t *data, size_t length, float timeout_secs) 00024 { 00025 float t = 0.0f; 00026 00027 uint8_t tmp[length + 1]; 00028 tmp[0] = reg; 00029 memcpy(&tmp[1], data, length); 00030 00031 while (i2c.write(baseAddress & BYTE(11111110), (char*)&tmp[0], length + 1) != 0) 00032 if ((t += retryDelay_secs) > timeout_secs) 00033 return false; 00034 else 00035 wait(retryDelay_secs); 00036 00037 return true; 00038 } 00039 00040 inline bool MPU6050::write(Register reg, uint8_t data, float timeout_secs) 00041 { 00042 float t = 0.0f; 00043 00044 uint8_t tmp[] = { (uint8_t)reg, data }; 00045 00046 while (i2c.write(baseAddress & BYTE(11111110), (char*)&tmp[0], 2) != 0) 00047 if ((t += retryDelay_secs) > timeout_secs) 00048 return false; 00049 else 00050 wait(retryDelay_secs); 00051 00052 return true; 00053 } 00054 00055 bool MPU6050::getAuxVDDIOLevel(AuxVDDIOLevel *level, float timeout_secs) 00056 { 00057 uint8_t tmp; 00058 if (this->read(REG_AUX_VDDIO, &tmp, timeout_secs)) 00059 { 00060 *level = static_cast<AuxVDDIOLevel>(tmp & AUX_VDDIO_MASK); 00061 return true; 00062 } 00063 00064 return false; 00065 } 00066 00067 bool MPU6050::setAuxVDDIOLevel(AuxVDDIOLevel level, float timeout_secs) 00068 { 00069 uint8_t tmp; 00070 return (this->read(REG_AUX_VDDIO, &tmp, timeout_secs)) && (this->write(REG_AUX_VDDIO, (tmp & (~AUX_VDDIO_MASK)) | level, timeout_secs)); 00071 } 00072 00073 bool MPU6050::getGyroSampleRateDivider(uint8_t *rateDivider, float timeout_secs) 00074 { 00075 return this->read(REG_SMPLRT_DIV, rateDivider, timeout_secs); 00076 } 00077 00078 bool MPU6050::setGyroSampleRateDivider(uint8_t rateDivider, float timeout_secs) 00079 { 00080 return this->write(REG_SMPLRT_DIV, rateDivider, timeout_secs); 00081 } 00082 00083 bool MPU6050::getExternalFrameSync(ExtFrameSync *frameSync, float timeout_secs) 00084 { 00085 uint8_t tmp; 00086 if (this->read(REG_CONFIG, &tmp, timeout_secs)) 00087 { 00088 *frameSync = static_cast<ExtFrameSync>(tmp & EXT_SYNC_MASK); 00089 return true; 00090 } 00091 00092 return false; 00093 } 00094 00095 bool MPU6050::setExternalFrameSync(ExtFrameSync frameSync, float timeout_secs) 00096 { 00097 uint8_t tmp; 00098 return (this->read(REG_CONFIG, &tmp, timeout_secs)) && (this->write(REG_CONFIG, (tmp & (~EXT_SYNC_MASK)) | frameSync, timeout_secs)); 00099 } 00100 00101 bool MPU6050::getDLPFBandwidth(DLPFBandwidth *bandwith, float timeout_secs) 00102 { 00103 uint8_t tmp; 00104 if (this->read(REG_CONFIG, &tmp, timeout_secs)) 00105 { 00106 *bandwith = static_cast<DLPFBandwidth>(tmp & DLPF_MASK); 00107 return true; 00108 } 00109 00110 return false; 00111 } 00112 00113 bool MPU6050::setDLPFBandwidth(DLPFBandwidth bandwith, float timeout_secs) 00114 { 00115 uint8_t tmp; 00116 return (this->read(REG_CONFIG, &tmp, timeout_secs)) && (this->write(REG_CONFIG, (tmp & (~DLPF_MASK)) | bandwith, timeout_secs)); 00117 } 00118 00119 bool MPU6050::getGyroXSelfTestEnabled(bool *enabled, float timeout_secs) 00120 { 00121 uint8_t tmp; 00122 if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) 00123 { 00124 *enabled = ((tmp & GYRO_X_ST_MASK) != 0); 00125 return true; 00126 } 00127 00128 return false; 00129 } 00130 00131 bool MPU6050::setGyroXSelfTestEnabled(bool enabled, float timeout_secs) 00132 { 00133 uint8_t tmp; 00134 if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) 00135 { 00136 tmp &= ~(GYRO_X_ST_MASK); 00137 return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_X_ST_MASK) : tmp, timeout_secs); 00138 } 00139 return false; 00140 } 00141 00142 bool MPU6050::getGyroYSelfTestEnabled(bool *enabled, float timeout_secs) 00143 { 00144 uint8_t tmp; 00145 if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) 00146 { 00147 *enabled = ((tmp & GYRO_Y_ST_MASK) != 0); 00148 return true; 00149 } 00150 00151 return false; 00152 } 00153 00154 bool MPU6050::setGyroYSelfTestEnabled(bool enabled, float timeout_secs) 00155 { 00156 uint8_t tmp; 00157 if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) 00158 { 00159 tmp &= ~(GYRO_Y_ST_MASK); 00160 return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Y_ST_MASK) : tmp, timeout_secs); 00161 } 00162 return false; 00163 } 00164 00165 bool MPU6050::getGyroZSelfTestEnabled(bool *enabled, float timeout_secs) 00166 { 00167 uint8_t tmp; 00168 if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) 00169 { 00170 *enabled = ((tmp & GYRO_Z_ST_MASK) != 0); 00171 return true; 00172 } 00173 00174 return false; 00175 } 00176 00177 bool MPU6050::setGyroZSelfTestEnabled(bool enabled, float timeout_secs) 00178 { 00179 uint8_t tmp; 00180 if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) 00181 { 00182 tmp &= ~(GYRO_Z_ST_MASK); 00183 return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Z_ST_MASK) : tmp, timeout_secs); 00184 } 00185 return false; 00186 } 00187 00188 bool MPU6050::getGyroRange(GyroRange *gyroRange, float timeout_secs) 00189 { 00190 uint8_t tmp; 00191 if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) 00192 { 00193 *gyroRange = static_cast<GyroRange>(tmp & GYRO_RANGE_MASK); 00194 return true; 00195 } 00196 00197 return false; 00198 } 00199 00200 bool MPU6050::setGyroRange(GyroRange gyroRange, float timeout_secs) 00201 { 00202 uint8_t tmp; 00203 return (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) && (this->write(REG_GYRO_CONFIG, (tmp & (~GYRO_RANGE_MASK)) | gyroRange, timeout_secs)); 00204 } 00205 00206 bool MPU6050::getAccelXSelfTestEnabled(bool *enabled, float timeout_secs) 00207 { 00208 uint8_t tmp; 00209 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00210 { 00211 *enabled = ((tmp & ACCEL_X_ST_MASK) != 0); 00212 return true; 00213 } 00214 00215 return false; 00216 } 00217 00218 bool MPU6050::setAccelXSelfTestEnabled(bool enabled, float timeout_secs) 00219 { 00220 uint8_t tmp; 00221 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00222 { 00223 tmp &= ~(ACCEL_X_ST_MASK); 00224 return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_X_ST_MASK) : tmp, timeout_secs); 00225 } 00226 return false; 00227 } 00228 00229 bool MPU6050::getAccelYSelfTestEnabled(bool *enabled, float timeout_secs) 00230 { 00231 uint8_t tmp; 00232 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00233 { 00234 *enabled = ((tmp & ACCEL_Y_ST_MASK) != 0); 00235 return true; 00236 } 00237 00238 return false; 00239 } 00240 00241 bool MPU6050::setAccelYSelfTestEnabled(bool enabled, float timeout_secs) 00242 { 00243 uint8_t tmp; 00244 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00245 { 00246 tmp &= ~(ACCEL_Y_ST_MASK); 00247 return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Y_ST_MASK) : tmp, timeout_secs); 00248 } 00249 return false; 00250 } 00251 00252 bool MPU6050::getAccelZSelfTestEnabled(bool *enabled, float timeout_secs) 00253 { 00254 uint8_t tmp; 00255 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00256 { 00257 *enabled = ((tmp & ACCEL_Z_ST_MASK) != 0); 00258 return true; 00259 } 00260 00261 return false; 00262 } 00263 00264 bool MPU6050::setAccelZSelfTestEnabled(bool enabled, float timeout_secs) 00265 { 00266 uint8_t tmp; 00267 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00268 { 00269 tmp &= ~(ACCEL_Z_ST_MASK); 00270 return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Z_ST_MASK) : tmp, timeout_secs); 00271 } 00272 return false; 00273 } 00274 00275 bool MPU6050::getAccelHPFCutOff(AccelHPFCutOff *frequency, float timeout_secs) 00276 { 00277 uint8_t tmp; 00278 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00279 { 00280 *frequency = static_cast<AccelHPFCutOff>(tmp & ACCEL_HPF_MASK); 00281 return true; 00282 } 00283 00284 return false; 00285 } 00286 00287 bool MPU6050::setAccelHPFCutOff(AccelHPFCutOff frequency, float timeout_secs) 00288 { 00289 uint8_t tmp; 00290 return (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) && (this->write(REG_ACCEL_CONFIG, (tmp & (~ACCEL_HPF_MASK)) | frequency, timeout_secs)); 00291 } 00292 00293 bool MPU6050::getAccelRange(AccelRange *accelRange, float timeout_secs) 00294 { 00295 uint8_t tmp; 00296 if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) 00297 { 00298 *accelRange = static_cast<AccelRange>(tmp & ACCEL_RANGE_MASK); 00299 return true; 00300 } 00301 00302 return false; 00303 } 00304 00305 bool MPU6050::setAccelRange(AccelRange accelRange, float timeout_secs) 00306 { 00307 uint8_t tmp; 00308 return (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) && (this->write(REG_ACCEL_CONFIG, (tmp & (~ACCEL_RANGE_MASK)) | accelRange, timeout_secs)); 00309 } 00310 00311 bool MPU6050::getFreefallDetectionThreshold(uint8_t *threshold, float timeout_secs) 00312 { 00313 return this->read(REG_FF_THR, threshold, timeout_secs); 00314 } 00315 00316 bool MPU6050::setFreefallDetectionThreshold(uint8_t threshold, float timeout_secs) 00317 { 00318 return this->write(REG_FF_THR, threshold, timeout_secs); 00319 } 00320 00321 bool MPU6050::getFreefallDetectionDuration(uint8_t *duration, float timeout_secs) 00322 { 00323 return this->read(REG_FF_DUR, duration, timeout_secs); 00324 } 00325 00326 bool MPU6050::setFreefallDetectionDuration(uint8_t duration, float timeout_secs) 00327 { 00328 return this->write(REG_FF_DUR, duration, timeout_secs); 00329 } 00330 00331 bool MPU6050::getMotionDetectionThreshold(uint8_t *threshold, float timeout_secs) 00332 { 00333 return this->read(REG_MOT_THR, threshold, timeout_secs); 00334 } 00335 00336 bool MPU6050::setMotionDetectionThreshold(uint8_t threshold, float timeout_secs) 00337 { 00338 return this->write(REG_MOT_THR, threshold, timeout_secs); 00339 } 00340 00341 bool MPU6050::getMotionDetectionDuration(uint8_t *duration, float timeout_secs) 00342 { 00343 return this->read(REG_MOT_DUR, duration, timeout_secs); 00344 } 00345 00346 bool MPU6050::setMotionDetectionDuration(uint8_t duration, float timeout_secs) 00347 { 00348 return this->write(REG_MOT_DUR, duration, timeout_secs); 00349 } 00350 00351 bool MPU6050::getZeroMotionDetectionThreshold(uint8_t *threshold, float timeout_secs) 00352 { 00353 return this->read(REG_ZRMOT_THR, threshold, timeout_secs); 00354 } 00355 00356 bool MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold, float timeout_secs) 00357 { 00358 return this->write(REG_ZRMOT_THR, threshold, timeout_secs); 00359 } 00360 00361 bool MPU6050::getZeroMotionDetectionDuration(uint8_t *duration, float timeout_secs) 00362 { 00363 return this->read(REG_ZRMOT_DUR, duration, timeout_secs); 00364 } 00365 00366 bool MPU6050::setZeroMotionDetectionDuration(uint8_t duration, float timeout_secs) 00367 { 00368 return this->write(REG_ZRMOT_DUR, duration, timeout_secs); 00369 } 00370 00371 bool MPU6050::getTempFIFOEnabled(bool *enabled, float timeout_secs) 00372 { 00373 uint8_t tmp; 00374 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00375 { 00376 *enabled = ((tmp & TEMP_FIFO_EN_MASK) != 0); 00377 return true; 00378 } 00379 00380 return false; 00381 } 00382 00383 bool MPU6050::setTempFIFOEnabled(bool enabled, float timeout_secs) 00384 { 00385 uint8_t tmp; 00386 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00387 { 00388 tmp &= ~(TEMP_FIFO_EN_MASK); 00389 return this->write(REG_FIFO_EN, enabled ? (tmp | TEMP_FIFO_EN_MASK) : tmp, timeout_secs); 00390 } 00391 return false; 00392 } 00393 00394 bool MPU6050::getGyroXFIFOEnabled(bool *enabled, float timeout_secs) 00395 { 00396 uint8_t tmp; 00397 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00398 { 00399 *enabled = ((tmp & GYRO_X_FIFO_EN_MASK) != 0); 00400 return true; 00401 } 00402 00403 return false; 00404 } 00405 00406 bool MPU6050::setGyroXFIFOEnabled(bool enabled, float timeout_secs) 00407 { 00408 uint8_t tmp; 00409 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00410 { 00411 tmp &= ~(GYRO_X_FIFO_EN_MASK); 00412 return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_X_FIFO_EN_MASK) : tmp, timeout_secs); 00413 } 00414 return false; 00415 } 00416 00417 bool MPU6050::getGyroYFIFOEnabled(bool *enabled, float timeout_secs) 00418 { 00419 uint8_t tmp; 00420 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00421 { 00422 *enabled = ((tmp & GYRO_Y_FIFO_EN_MASK) != 0); 00423 return true; 00424 } 00425 00426 return false; 00427 } 00428 00429 bool MPU6050::setGyroYFIFOEnabled(bool enabled, float timeout_secs) 00430 { 00431 uint8_t tmp; 00432 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00433 { 00434 tmp &= ~(GYRO_Y_FIFO_EN_MASK); 00435 return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Y_FIFO_EN_MASK) : tmp, timeout_secs); 00436 } 00437 return false; 00438 } 00439 00440 bool MPU6050::getGyroZFIFOEnabled(bool *enabled, float timeout_secs) 00441 { 00442 uint8_t tmp; 00443 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00444 { 00445 *enabled = ((tmp & GYRO_Z_FIFO_EN_MASK) != 0); 00446 return true; 00447 } 00448 00449 return false; 00450 } 00451 00452 bool MPU6050::setGyroZFIFOEnabled(bool enabled, float timeout_secs) 00453 { 00454 uint8_t tmp; 00455 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00456 { 00457 tmp &= ~(GYRO_Z_FIFO_EN_MASK); 00458 return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Z_FIFO_EN_MASK) : tmp, timeout_secs); 00459 } 00460 return false; 00461 } 00462 00463 bool MPU6050::getAccelFIFOEnabled(bool *enabled, float timeout_secs) 00464 { 00465 uint8_t tmp; 00466 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00467 { 00468 *enabled = ((tmp & ACCEL_FIFO_EN_MASK) != 0); 00469 return true; 00470 } 00471 00472 return false; 00473 } 00474 00475 bool MPU6050::setAccelFIFOEnabled(bool enabled, float timeout_secs) 00476 { 00477 uint8_t tmp; 00478 if (this->read(REG_FIFO_EN, &tmp, timeout_secs)) 00479 { 00480 tmp &= ~(ACCEL_FIFO_EN_MASK); 00481 return this->write(REG_FIFO_EN, enabled ? (tmp | ACCEL_FIFO_EN_MASK) : tmp, timeout_secs); 00482 } 00483 return false; 00484 } 00485 00486 bool MPU6050::getInterruptLevel(InterruptLevel *level, float timeout_secs) 00487 { 00488 uint8_t tmp; 00489 if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) 00490 { 00491 *level = static_cast<InterruptLevel>(tmp & INT_LEVEL_MASK); 00492 return true; 00493 } 00494 00495 return false; 00496 } 00497 00498 bool MPU6050::setInterruptLevel(InterruptLevel level, float timeout_secs) 00499 { 00500 uint8_t tmp; 00501 return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_LEVEL_MASK)) | level, timeout_secs)); 00502 } 00503 00504 bool MPU6050::getInterruptDrive(InterruptDrive *drive, float timeout_secs) 00505 { 00506 uint8_t tmp; 00507 if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) 00508 { 00509 *drive = static_cast<InterruptDrive>(tmp & INT_DRIVE_MASK); 00510 return true; 00511 } 00512 00513 return false; 00514 } 00515 00516 bool MPU6050::setInterruptDrive(InterruptDrive drive, float timeout_secs) 00517 { 00518 uint8_t tmp; 00519 return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_DRIVE_MASK)) | drive, timeout_secs)); 00520 } 00521 00522 bool MPU6050::getInterruptLatch(InterruptLatch *latch, float timeout_secs) 00523 { 00524 uint8_t tmp; 00525 if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) 00526 { 00527 *latch = static_cast<InterruptLatch>(tmp & INT_LATCH_MASK); 00528 return true; 00529 } 00530 00531 return false; 00532 } 00533 00534 bool MPU6050::setInterruptLatch(InterruptLatch latch, float timeout_secs) 00535 { 00536 uint8_t tmp; 00537 return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_LATCH_MASK)) | latch, timeout_secs)); 00538 } 00539 00540 bool MPU6050::getInterruptLatchClear(InterruptClear *latchClear, float timeout_secs) 00541 { 00542 uint8_t tmp; 00543 if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) 00544 { 00545 *latchClear = static_cast<InterruptClear>(tmp & INT_CLEAR_MASK); 00546 return true; 00547 } 00548 00549 return false; 00550 } 00551 00552 bool MPU6050::setInterruptLatchClear(InterruptClear latchClear, float timeout_secs) 00553 { 00554 uint8_t tmp; 00555 return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_CLEAR_MASK)) | latchClear, timeout_secs)); 00556 } 00557 00558 bool MPU6050::getInterruptFSyncLevel(InterruptFSyncLevel *fsyncLevel, float timeout_secs) 00559 { 00560 uint8_t tmp; 00561 if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) 00562 { 00563 *fsyncLevel = static_cast<InterruptFSyncLevel>(tmp & INT_FSYNC_LEVEL_MASK); 00564 return true; 00565 } 00566 00567 return false; 00568 } 00569 00570 bool MPU6050::setInterruptFSyncLevel(InterruptFSyncLevel fsyncLevel, float timeout_secs) 00571 { 00572 uint8_t tmp; 00573 return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_FSYNC_LEVEL_MASK)) | fsyncLevel, timeout_secs)); 00574 } 00575 00576 bool MPU6050::getInterruptFSyncEnabled(bool *enabled, float timeout_secs) 00577 { 00578 uint8_t tmp; 00579 if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) 00580 { 00581 *enabled = ((tmp & INT_FSYNC_EN_MASK) != 0); 00582 return true; 00583 } 00584 00585 return false; 00586 } 00587 00588 bool MPU6050::setInterruptFSyncEnabled(bool enabled, float timeout_secs) 00589 { 00590 uint8_t tmp; 00591 if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) 00592 { 00593 tmp &= ~(INT_FSYNC_EN_MASK); 00594 return this->write(REG_FIFO_EN, enabled ? (tmp | INT_FSYNC_EN_MASK) : tmp, timeout_secs); 00595 } 00596 return false; 00597 } 00598 00599 bool MPU6050::getInterruptsEnabled(Interrupt *interruptSet, float timeout_secs) 00600 { 00601 uint8_t tmp; 00602 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00603 { 00604 *interruptSet = static_cast<Interrupt>(tmp); 00605 return true; 00606 } 00607 return false; 00608 } 00609 00610 bool MPU6050::setInterruptsEnabled(Interrupt interruptSet, float timeout_secs) 00611 { 00612 uint8_t tmp = interruptSet; 00613 return this->write(REG_INT_ENABLE, tmp, timeout_secs); 00614 } 00615 00616 bool MPU6050::getInterruptFreefallEnabled(bool *enabled, float timeout_secs) 00617 { 00618 uint8_t tmp; 00619 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00620 { 00621 *enabled = ((tmp & INT_FREEFALL) != 0); 00622 return true; 00623 } 00624 00625 return false; 00626 } 00627 00628 bool MPU6050::setInterruptFreefallEnabled(bool enabled, float timeout_secs) 00629 { 00630 uint8_t tmp; 00631 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00632 { 00633 tmp &= ~(INT_FREEFALL); 00634 return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FREEFALL) : tmp, timeout_secs); 00635 } 00636 return false; 00637 } 00638 00639 bool MPU6050::getInterruptMotionEnabled(bool *enabled, float timeout_secs) 00640 { 00641 uint8_t tmp; 00642 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00643 { 00644 *enabled = ((tmp & INT_MOTION) != 0); 00645 return true; 00646 } 00647 00648 return false; 00649 } 00650 00651 bool MPU6050::setInterruptMotionEnabled(bool enabled, float timeout_secs) 00652 { 00653 uint8_t tmp; 00654 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00655 { 00656 tmp &= ~(INT_MOTION); 00657 return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_MOTION) : tmp, timeout_secs); 00658 } 00659 return false; 00660 } 00661 00662 bool MPU6050::getInterruptZeroMotionEnabled(bool *enabled, float timeout_secs) 00663 { 00664 uint8_t tmp; 00665 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00666 { 00667 *enabled = ((tmp & INT_ZERO_MOTION) != 0); 00668 return true; 00669 } 00670 00671 return false; 00672 } 00673 00674 bool MPU6050::setInterruptZeroMotionEnabled(bool enabled, float timeout_secs) 00675 { 00676 uint8_t tmp; 00677 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00678 { 00679 tmp &= ~(INT_ZERO_MOTION); 00680 return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_ZERO_MOTION) : tmp, timeout_secs); 00681 } 00682 return false; 00683 } 00684 00685 bool MPU6050::getInterruptFIFOOverflowEnabled(bool *enabled, float timeout_secs) 00686 { 00687 uint8_t tmp; 00688 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00689 { 00690 *enabled = ((tmp & INT_FIFO_OVERFLOW) != 0); 00691 return true; 00692 } 00693 00694 return false; 00695 } 00696 00697 bool MPU6050::setInterruptFIFOOverflowEnabled(bool enabled, float timeout_secs) 00698 { 00699 uint8_t tmp; 00700 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00701 { 00702 tmp &= ~(INT_FIFO_OVERFLOW); 00703 return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FIFO_OVERFLOW) : tmp, timeout_secs); 00704 } 00705 return false; 00706 } 00707 00708 bool MPU6050::getInterruptDataReadyEnabled(bool *enabled, float timeout_secs) 00709 { 00710 uint8_t tmp; 00711 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00712 { 00713 *enabled = ((tmp & INT_DATA_READY) != 0); 00714 return true; 00715 } 00716 00717 return false; 00718 } 00719 00720 bool MPU6050::setInterruptDataReadyEnabled(bool enabled, float timeout_secs) 00721 { 00722 uint8_t tmp; 00723 if (this->read(REG_INT_ENABLE, &tmp, timeout_secs)) 00724 { 00725 tmp &= ~(INT_DATA_READY); 00726 return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_DATA_READY) : tmp, timeout_secs); 00727 } 00728 return false; 00729 } 00730 00731 bool MPU6050::getInterruptStatuses(Interrupt *interruptSet, float timeout_secs) 00732 { 00733 uint8_t tmp; 00734 if (this->read(REG_INT_STATUS, &tmp, timeout_secs)) 00735 { 00736 *interruptSet = static_cast<Interrupt>(tmp); 00737 return true; 00738 } 00739 return false; 00740 } 00741 00742 bool MPU6050::getInterruptFreefallStatus(bool *status, float timeout_secs) 00743 { 00744 uint8_t tmp; 00745 if (this->read(REG_INT_STATUS, &tmp, timeout_secs)) 00746 { 00747 *status = ((tmp & INT_FREEFALL) != 0); 00748 return true; 00749 } 00750 00751 return false; 00752 } 00753 00754 bool MPU6050::getInterruptMotionStatus(bool *status, float timeout_secs) 00755 { 00756 uint8_t tmp; 00757 if (this->read(REG_INT_STATUS, &tmp, timeout_secs)) 00758 { 00759 *status = ((tmp & INT_MOTION) != 0); 00760 return true; 00761 } 00762 00763 return false; 00764 } 00765 00766 bool MPU6050::getInterruptZeroMotionStatus(bool *status, float timeout_secs) 00767 { 00768 uint8_t tmp; 00769 if (this->read(REG_INT_STATUS, &tmp, timeout_secs)) 00770 { 00771 *status = ((tmp & INT_ZERO_MOTION) != 0); 00772 return true; 00773 } 00774 00775 return false; 00776 } 00777 00778 bool MPU6050::getInterruptFIFOOverflowStatus(bool *status, float timeout_secs) 00779 { 00780 uint8_t tmp; 00781 if (this->read(REG_INT_STATUS, &tmp, timeout_secs)) 00782 { 00783 *status = ((tmp & INT_FIFO_OVERFLOW) != 0); 00784 return true; 00785 } 00786 00787 return false; 00788 } 00789 00790 bool MPU6050::getInterruptDataReadyStatus(bool *status, float timeout_secs) 00791 { 00792 uint8_t tmp; 00793 if (this->read(REG_INT_STATUS, &tmp, timeout_secs)) 00794 { 00795 *status = ((tmp & INT_DATA_READY) != 0); 00796 return true; 00797 } 00798 00799 return false; 00800 } 00801 00802 bool MPU6050::getAcceleration(int16_t *ax, int16_t *ay, int16_t *az, float timeout_secs) 00803 { 00804 uint8_t tmp[6]; 00805 if (this->read(REG_ACCEL_XOUT_H, tmp, 6, timeout_secs)) 00806 { 00807 *ax = (int16_t)((tmp[0] << 8) | tmp[1]); 00808 *ay = (int16_t)((tmp[2] << 8) | tmp[3]); 00809 *az = (int16_t)((tmp[4] << 8) | tmp[5]); 00810 00811 return true; 00812 } 00813 00814 return false; 00815 } 00816 00817 bool MPU6050::getAccelerationX(int16_t *ax, float timeout_secs) 00818 { 00819 uint8_t tmp[2]; 00820 if (this->read(REG_ACCEL_XOUT_H, tmp, 2, timeout_secs)) 00821 { 00822 *ax = (int16_t)((tmp[0] << 8) | tmp[1]); 00823 return true; 00824 } 00825 00826 return false; 00827 } 00828 00829 bool MPU6050::getAccelerationY(int16_t *ay, float timeout_secs) 00830 { 00831 uint8_t tmp[2]; 00832 if (this->read(REG_ACCEL_YOUT_H, tmp, 2, timeout_secs)) 00833 { 00834 *ay = (int16_t)((tmp[0] << 8) | tmp[1]); 00835 return true; 00836 } 00837 00838 return false; 00839 } 00840 00841 bool MPU6050::getAccelerationZ(int16_t *az, float timeout_secs) 00842 { 00843 uint8_t tmp[2]; 00844 if (this->read(REG_ACCEL_ZOUT_H, tmp, 2, timeout_secs)) 00845 { 00846 *az = (int16_t)((tmp[0] << 8) | tmp[1]); 00847 return true; 00848 } 00849 00850 return false; 00851 } 00852 00853 bool MPU6050::getTemperature(int16_t *temp, float timeout_secs) 00854 { 00855 uint8_t tmp[2]; 00856 if (this->read(REG_TEMP_OUT_H, tmp, 2, timeout_secs)) 00857 { 00858 *temp = (int16_t)((tmp[0] << 8) | tmp[1]); 00859 return true; 00860 } 00861 00862 return false; 00863 } 00864 00865 bool MPU6050::getRotationSpeed(int16_t *ox, int16_t *oy, int16_t *oz, float timeout_secs) 00866 { 00867 uint8_t tmp[6]; 00868 if (this->read(REG_GYRO_XOUT_H, tmp, 6, timeout_secs)) 00869 { 00870 *ox = (int16_t)((tmp[0] << 8) | tmp[1]); 00871 *oy = (int16_t)((tmp[2] << 8) | tmp[3]); 00872 *oz = (int16_t)((tmp[4] << 8) | tmp[5]); 00873 00874 return true; 00875 } 00876 00877 return false; 00878 } 00879 00880 bool MPU6050::getRotationSpeedX(int16_t *ox, float timeout_secs) 00881 { 00882 uint8_t tmp[2]; 00883 if (this->read(REG_GYRO_XOUT_H, tmp, 2, timeout_secs)) 00884 { 00885 *ox = (int16_t)((tmp[0] << 8) | tmp[1]); 00886 return true; 00887 } 00888 00889 return false; 00890 } 00891 00892 bool MPU6050::getRotationSpeedY(int16_t *oy, float timeout_secs) 00893 { 00894 uint8_t tmp[2]; 00895 if (this->read(REG_GYRO_YOUT_H, tmp, 2, timeout_secs)) 00896 { 00897 *oy = (int16_t)((tmp[0] << 8) | tmp[1]); 00898 return true; 00899 } 00900 00901 return false; 00902 } 00903 00904 bool MPU6050::getRotationSpeedZ(int16_t *oz, float timeout_secs) 00905 { 00906 uint8_t tmp[2]; 00907 if (this->read(REG_GYRO_ZOUT_H, tmp, 2, timeout_secs)) 00908 { 00909 *oz = (int16_t)((tmp[0] << 8) | tmp[1]); 00910 return true; 00911 } 00912 00913 return false; 00914 } 00915 00916 bool MPU6050::getMotionAndTemperature(int16_t *ax, int16_t *ay, int16_t *az, int16_t *temp, int16_t *ox, int16_t *oy, int16_t *oz, float timeout_secs) 00917 { 00918 uint8_t tmp[14]; 00919 if (this->read(REG_ACCEL_XOUT_H, &tmp[0], 14, timeout_secs)) 00920 { 00921 *ax = (int16_t)(((uint16_t)tmp[0] << 8) | tmp[1]); 00922 *ay = (int16_t)(((uint16_t)tmp[2] << 8) | tmp[3]); 00923 *az = (int16_t)(((uint16_t)tmp[4] << 8) | tmp[5]); 00924 *temp = (int16_t)(((uint16_t)tmp[6] << 8) | tmp[7]); 00925 *ox = (int16_t)(((uint16_t)tmp[8] << 8) | tmp[9]); 00926 *oy = (int16_t)(((uint16_t)tmp[10] << 8) | tmp[11]); 00927 *oz = (int16_t)(((uint16_t)tmp[12] << 8) | tmp[13]); 00928 00929 return true; 00930 } 00931 00932 return false; 00933 } 00934 00935 bool MPU6050::getMotionAndTemperature(SensorReading *sensorReading, float timeout_secs) 00936 { 00937 uint8_t tmp; 00938 #define SWAP(a, b) tmp = a; a = b; b = tmp; 00939 00940 if (this->read(REG_ACCEL_XOUT_H, (uint8_t*)&converter, sizeof(converter), timeout_secs)) 00941 { 00942 SWAP(converter.reg.ax_h, converter.reg.ax_l); 00943 SWAP(converter.reg.ay_h, converter.reg.ay_l); 00944 SWAP(converter.reg.az_h, converter.reg.az_l); 00945 SWAP(converter.reg.temp_h, converter.reg.temp_l); 00946 SWAP(converter.reg.ox_h, converter.reg.ox_l); 00947 SWAP(converter.reg.oy_h, converter.reg.oy_l); 00948 SWAP(converter.reg.oz_h, converter.reg.oz_l); 00949 00950 *sensorReading = converter.value; 00951 00952 return true; 00953 } 00954 00955 return false; 00956 00957 #undef SWAP 00958 } 00959 00960 bool MPU6050::getMotionStatus(Motion *motionSet, float timeout_secs) 00961 { 00962 uint8_t tmp; 00963 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 00964 { 00965 *motionSet = static_cast<Motion>(tmp); 00966 return true; 00967 } 00968 return false; 00969 } 00970 00971 bool MPU6050::getNegativeXMotionDetected(bool *detected, float timeout_secs) 00972 { 00973 uint8_t tmp; 00974 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 00975 { 00976 *detected = ((tmp & MOT_NEGATIVE_X) != 0); 00977 return true; 00978 } 00979 00980 return false; 00981 } 00982 00983 bool MPU6050::getPositiveXMotionDetected(bool *detected, float timeout_secs) 00984 { 00985 uint8_t tmp; 00986 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 00987 { 00988 *detected = ((tmp & MOT_POSITIVE_X) != 0); 00989 return true; 00990 } 00991 00992 return false; 00993 } 00994 00995 bool MPU6050::getNegativeYMotionDetected(bool *detected, float timeout_secs) 00996 { 00997 uint8_t tmp; 00998 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 00999 { 01000 *detected = ((tmp & MOT_NEGATIVE_Y) != 0); 01001 return true; 01002 } 01003 01004 return false; 01005 } 01006 01007 bool MPU6050::getPositiveYMotionDetected(bool *detected, float timeout_secs) 01008 { 01009 uint8_t tmp; 01010 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 01011 { 01012 *detected = ((tmp & MOT_POSITIVE_Y) != 0); 01013 return true; 01014 } 01015 01016 return false; 01017 } 01018 01019 bool MPU6050::getNegativeZMotionDetected(bool *detected, float timeout_secs) 01020 { 01021 uint8_t tmp; 01022 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 01023 { 01024 *detected = ((tmp & MOT_NEGATIVE_Z) != 0); 01025 return true; 01026 } 01027 01028 return false; 01029 } 01030 01031 bool MPU6050::getPositiveZMotionDetected(bool *detected, float timeout_secs) 01032 { 01033 uint8_t tmp; 01034 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 01035 { 01036 *detected = ((tmp & MOT_POSITIVE_Z) != 0); 01037 return true; 01038 } 01039 01040 return false; 01041 } 01042 01043 bool MPU6050::getZeroMotionDetected(bool *detected, float timeout_secs) 01044 { 01045 uint8_t tmp; 01046 if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs)) 01047 { 01048 *detected = ((tmp & MOT_ZERO) != 0); 01049 return true; 01050 } 01051 01052 return false; 01053 } 01054 01055 bool MPU6050::resetGyroscopeSignalPath(float timeout_secs) 01056 { 01057 uint8_t tmp; 01058 return (this->read(REG_SIGNAL_PATH_RESET, &tmp) && this->write(REG_SIGNAL_PATH_RESET, tmp | GYRO_PATH_RESET_MASK)); 01059 } 01060 01061 bool MPU6050::resetAccelerometerSignalPath(float timeout_secs) 01062 { 01063 uint8_t tmp; 01064 return (this->read(REG_SIGNAL_PATH_RESET, &tmp) && this->write(REG_SIGNAL_PATH_RESET, tmp | ACCEL_PATH_RESET_MASK)); 01065 } 01066 01067 bool MPU6050::resetTemperatureSignalPath(float timeout_secs) 01068 { 01069 uint8_t tmp; 01070 return (this->read(REG_SIGNAL_PATH_RESET, &tmp) && this->write(REG_SIGNAL_PATH_RESET, tmp | TEMP_PATH_RESET_MASK)); 01071 } 01072 01073 bool MPU6050::getAccelerometerPowerOnDelay(PowerOnDelay* delay, float timeout_secs) 01074 { 01075 uint8_t tmp; 01076 if (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) 01077 { 01078 *delay = static_cast<PowerOnDelay>(tmp & POWER_ON_DELAY_MASK); 01079 return true; 01080 } 01081 01082 return false; 01083 } 01084 01085 bool MPU6050::setAccelerometerPowerOnDelay(PowerOnDelay delay, float timeout_secs) 01086 { 01087 uint8_t tmp; 01088 return (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) && (this->write(REG_MOT_DETECT_CTRL, (tmp & (~POWER_ON_DELAY_MASK)) | delay, timeout_secs)); 01089 } 01090 01091 bool MPU6050::getFreefallDetectionDecrement(FreefallDetectionDecrement* dec, float timeout_secs) 01092 { 01093 uint8_t tmp; 01094 if (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) 01095 { 01096 *dec = static_cast<FreefallDetectionDecrement>(tmp & FF_DETECTION_DEC_MASK); 01097 return true; 01098 } 01099 01100 return false; 01101 } 01102 01103 bool MPU6050::setFreefallDetectionDecrement(FreefallDetectionDecrement dec, float timeout_secs) 01104 { 01105 uint8_t tmp; 01106 return (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) && (this->write(REG_MOT_DETECT_CTRL, (tmp & (~FF_DETECTION_DEC_MASK)) | dec, timeout_secs)); 01107 } 01108 01109 bool MPU6050::getMotionDetectionDecrement(MotionDetectionDecrement* dec, float timeout_secs) 01110 { 01111 uint8_t tmp; 01112 if (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) 01113 { 01114 *dec = static_cast<MotionDetectionDecrement>(tmp & MOT_DETECTION_DEC_MASK); 01115 return true; 01116 } 01117 01118 return false; 01119 } 01120 01121 bool MPU6050::setMotionDetectionDecrement(MotionDetectionDecrement dec, float timeout_secs) 01122 { 01123 uint8_t tmp; 01124 return (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) && (this->write(REG_MOT_DETECT_CTRL, (tmp & (~MOT_DETECTION_DEC_MASK)) | dec, timeout_secs)); 01125 } 01126 01127 bool MPU6050::getFIFOEnabled(bool *enabled, float timeout_secs) 01128 { 01129 uint8_t tmp; 01130 if (this->read(REG_USER_CTRL, &tmp, timeout_secs)) 01131 { 01132 *enabled = ((tmp & FIFO_EN_MASK) != 0); 01133 return true; 01134 } 01135 01136 return false; 01137 } 01138 01139 bool MPU6050::setFIFOEnabled(bool enabled, float timeout_secs) 01140 { 01141 uint8_t tmp; 01142 if (this->read(REG_USER_CTRL, &tmp, timeout_secs)) 01143 { 01144 tmp &= ~(FIFO_EN_MASK); 01145 return this->write(REG_USER_CTRL, enabled ? (tmp | FIFO_EN_MASK) : tmp, timeout_secs); 01146 } 01147 return false; 01148 } 01149 01150 bool MPU6050::resetFIFO(float timeout_secs) 01151 { 01152 uint8_t tmp; 01153 return (this->read(REG_USER_CTRL, &tmp) && this->write(REG_USER_CTRL, tmp | FIFO_RESET_MASK)); 01154 } 01155 01156 bool MPU6050::resetSensors(float timeout_secs) 01157 { 01158 uint8_t tmp; 01159 return (this->read(REG_USER_CTRL, &tmp) && this->write(REG_USER_CTRL, tmp | SIG_COND_RESET_MASK)); 01160 } 01161 01162 bool MPU6050::reset(float timeout_secs) 01163 { 01164 uint8_t tmp; 01165 return (this->read(REG_PWR_MGMT_1, &tmp) && this->write(REG_PWR_MGMT_1, tmp | RESET_MASK)); 01166 } 01167 01168 bool MPU6050::getSleepEnabled(bool *enabled, float timeout_secs) 01169 { 01170 uint8_t tmp; 01171 if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) 01172 { 01173 *enabled = ((tmp & SLEEP_EN_MASK) != 0); 01174 return true; 01175 } 01176 01177 return false; 01178 } 01179 01180 bool MPU6050::setSleepEnabled(bool enabled, float timeout_secs) 01181 { 01182 uint8_t tmp; 01183 if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) 01184 { 01185 tmp &= ~(SLEEP_EN_MASK); 01186 return this->write(REG_PWR_MGMT_1, enabled ? (tmp | SLEEP_EN_MASK) : tmp, timeout_secs); 01187 } 01188 return false; 01189 } 01190 01191 bool MPU6050::getWakeCycleEnabled(bool *enabled, float timeout_secs) 01192 { 01193 uint8_t tmp; 01194 if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) 01195 { 01196 *enabled = ((tmp & WAKE_CYCLE_EN_MASK) != 0); 01197 return true; 01198 } 01199 01200 return false; 01201 } 01202 01203 bool MPU6050::setWakeCycleEnabled(bool enabled, float timeout_secs) 01204 { 01205 uint8_t tmp; 01206 if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) 01207 { 01208 tmp &= ~(WAKE_CYCLE_EN_MASK); 01209 return this->write(REG_PWR_MGMT_1, enabled ? (tmp | WAKE_CYCLE_EN_MASK) : tmp, timeout_secs); 01210 } 01211 return false; 01212 } 01213 01214 bool MPU6050::getTemperatureSensorEnabled(bool *enabled, float timeout_secs) 01215 { 01216 uint8_t tmp; 01217 if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) 01218 { 01219 // this is an inverted state 01220 *enabled = ((tmp & TEMP_EN_MASK) == 0); 01221 return true; 01222 } 01223 01224 return false; 01225 } 01226 01227 bool MPU6050::setTemperatureSensorEnabled(bool enabled, float timeout_secs) 01228 { 01229 uint8_t tmp; 01230 if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) 01231 { 01232 tmp &= ~(TEMP_EN_MASK); 01233 //this is an incverted state 01234 return this->write(REG_PWR_MGMT_1, enabled ? tmp : (tmp | TEMP_EN_MASK), timeout_secs); 01235 } 01236 return false; 01237 } 01238 01239 bool MPU6050::getClockSource(ClockSource* clockSource, float timeout_secs) 01240 { 01241 uint8_t tmp; 01242 if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) 01243 { 01244 *clockSource = static_cast<ClockSource>(tmp & CLOCK_SOURCE_MASK); 01245 return true; 01246 } 01247 01248 return false; 01249 } 01250 01251 bool MPU6050::setClockSource(ClockSource clockSource, float timeout_secs) 01252 { 01253 uint8_t tmp; 01254 return (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) && (this->write(REG_PWR_MGMT_1, (tmp & (~CLOCK_SOURCE_MASK)) | clockSource, timeout_secs)); 01255 } 01256 01257 bool MPU6050::getWakeFrequency(WakeFrequency* wakeFrequency, float timeout_secs) 01258 { 01259 uint8_t tmp; 01260 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01261 { 01262 *wakeFrequency = static_cast<WakeFrequency>(tmp & WAKE_FREQ_MASK); 01263 return true; 01264 } 01265 01266 return false; 01267 } 01268 01269 bool MPU6050::setWakeFrequency(WakeFrequency wakeFrequency, float timeout_secs) 01270 { 01271 uint8_t tmp; 01272 return (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) && (this->write(REG_PWR_MGMT_2, (tmp & (~WAKE_FREQ_MASK)) | wakeFrequency, timeout_secs)); 01273 } 01274 01275 bool MPU6050::getGyroXStandbyEnabled(bool *enabled, float timeout_secs) 01276 { 01277 uint8_t tmp; 01278 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01279 { 01280 *enabled = ((tmp & GYRO_X_STB_EN_MASK) != 0); 01281 return true; 01282 } 01283 01284 return false; 01285 } 01286 01287 bool MPU6050::setGyroXStandbyEnabled(bool enabled, float timeout_secs) 01288 { 01289 uint8_t tmp; 01290 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01291 { 01292 tmp &= ~(GYRO_X_STB_EN_MASK); 01293 return this->write(REG_PWR_MGMT_2, enabled ? (tmp | GYRO_X_STB_EN_MASK) : tmp, timeout_secs); 01294 } 01295 return false; 01296 } 01297 01298 bool MPU6050::getGyroYStandbyEnabled(bool *enabled, float timeout_secs) 01299 { 01300 uint8_t tmp; 01301 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01302 { 01303 *enabled = ((tmp & GYRO_Y_STB_EN_MASK) != 0); 01304 return true; 01305 } 01306 01307 return false; 01308 } 01309 01310 bool MPU6050::setGyroYStandbyEnabled(bool enabled, float timeout_secs) 01311 { 01312 uint8_t tmp; 01313 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01314 { 01315 tmp &= ~(GYRO_Y_STB_EN_MASK); 01316 return this->write(REG_PWR_MGMT_2, enabled ? (tmp | GYRO_Y_STB_EN_MASK) : tmp, timeout_secs); 01317 } 01318 return false; 01319 } 01320 01321 bool MPU6050::getGyroZStandbyEnabled(bool *enabled, float timeout_secs) 01322 { 01323 uint8_t tmp; 01324 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01325 { 01326 *enabled = ((tmp & GYRO_Z_STB_EN_MASK) != 0); 01327 return true; 01328 } 01329 01330 return false; 01331 } 01332 01333 bool MPU6050::setGyroZStandbyEnabled(bool enabled, float timeout_secs) 01334 { 01335 uint8_t tmp; 01336 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01337 { 01338 tmp &= ~(GYRO_Z_STB_EN_MASK); 01339 return this->write(REG_PWR_MGMT_2, enabled ? (tmp | GYRO_Z_STB_EN_MASK) : tmp, timeout_secs); 01340 } 01341 return false; 01342 } 01343 01344 bool MPU6050::getAccelXStandbyEnabled(bool *enabled, float timeout_secs) 01345 { 01346 uint8_t tmp; 01347 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01348 { 01349 *enabled = ((tmp & ACCEL_X_STB_EN_MASK) != 0); 01350 return true; 01351 } 01352 01353 return false; 01354 } 01355 01356 bool MPU6050::setAccelXStandbyEnabled(bool enabled, float timeout_secs) 01357 { 01358 uint8_t tmp; 01359 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01360 { 01361 tmp &= ~(ACCEL_X_STB_EN_MASK); 01362 return this->write(REG_PWR_MGMT_2, enabled ? (tmp | ACCEL_X_STB_EN_MASK) : tmp, timeout_secs); 01363 } 01364 return false; 01365 } 01366 01367 bool MPU6050::getAccelYStandbyEnabled(bool *enabled, float timeout_secs) 01368 { 01369 uint8_t tmp; 01370 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01371 { 01372 *enabled = ((tmp & ACCEL_Y_STB_EN_MASK) != 0); 01373 return true; 01374 } 01375 01376 return false; 01377 } 01378 01379 bool MPU6050::setAccelYStandbyEnabled(bool enabled, float timeout_secs) 01380 { 01381 uint8_t tmp; 01382 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01383 { 01384 tmp &= ~(ACCEL_Y_STB_EN_MASK); 01385 return this->write(REG_PWR_MGMT_2, enabled ? (tmp | ACCEL_Y_STB_EN_MASK) : tmp, timeout_secs); 01386 } 01387 return false; 01388 } 01389 01390 bool MPU6050::getAccelZStandbyEnabled(bool *enabled, float timeout_secs) 01391 { 01392 uint8_t tmp; 01393 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01394 { 01395 *enabled = ((tmp & ACCEL_Z_STB_EN_MASK) != 0); 01396 return true; 01397 } 01398 01399 return false; 01400 } 01401 01402 bool MPU6050::setAccelZStandbyEnabled(bool enabled, float timeout_secs) 01403 { 01404 uint8_t tmp; 01405 if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) 01406 { 01407 tmp &= ~(ACCEL_Z_STB_EN_MASK); 01408 return this->write(REG_PWR_MGMT_2, enabled ? (tmp | ACCEL_Z_STB_EN_MASK) : tmp, timeout_secs); 01409 } 01410 return false; 01411 } 01412 01413 bool MPU6050::getFIFOCount(uint16_t *count, float timeout_secs) 01414 { 01415 uint8_t tmp[2]; 01416 if (this->read(REG_FIFO_COUNTH, tmp, 2, timeout_secs)) 01417 { 01418 *count = (((int16_t)tmp[0]) << 8) | tmp[1]; 01419 return true; 01420 } 01421 01422 return false; 01423 } 01424 01425 bool MPU6050::readFIFO(uint8_t *data, float timeout_secs) 01426 { 01427 return this->read(REG_FIFO_R_W, data, timeout_secs); 01428 } 01429 01430 bool MPU6050::writeFIFO(uint8_t data, float timeout_secs) 01431 { 01432 return this->write(REG_FIFO_R_W, data, timeout_secs); 01433 } 01434 01435 bool MPU6050::readFIFO(uint8_t *data, size_t lenght, float timeout_secs) 01436 { 01437 return this->read(REG_FIFO_R_W, data, lenght, timeout_secs); 01438 } 01439 01440 bool MPU6050::writeFIFO(uint8_t *data, size_t lenght, float timeout_secs) 01441 { 01442 return this->write(REG_FIFO_R_W, data, lenght, timeout_secs); 01443 } 01444 01445 bool MPU6050::getDeviceId(uint8_t *id, float timeout_secs) 01446 { 01447 uint8_t tmp; 01448 if (this->read(REG_WHO_AM_I, &tmp, timeout_secs)) 01449 { 01450 *id = tmp & DEVICE_ID_MASK; 01451 return true; 01452 } 01453 01454 return false; 01455 }
Generated on Fri Jul 15 2022 02:47:31 by
1.7.2