File MpuController.cpp
File List > fw > rbcx-coprocessor > src > MpuController.cpp
Go to the documentation of this file.
// I2Cdev library collection - mpu I2C device class
// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
// 8/24/2011 by Jeff Rowberg <jeff@rowberg.net>
// 11/28/2014 by Marton Sebok <sebokmarton@gmail.com>
//
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// ... - ongoing debug release
// 2014-11-28 - ported to PIC18 peripheral library from Arduino code
// 2017-03-28 - tested basic function on STM32
// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF
// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING.
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
Copyright (c) 2014 Marton Sebok
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
#include "MpuController.hpp"
#include "Dispatcher.hpp"
#include "utils/TickTimer.hpp"
#include "Bsp.hpp"
#include "utils/QueueWrapper.hpp"
#include "utils/TaskWrapper.hpp"
#include "FreeRTOS.h"
#include "timers.h"
#include "event_groups.h"
typedef struct MpuVector {
int16_t x;
int16_t y;
int16_t z;
} MpuVector;
typedef struct MpuVector32 {
int32_t x;
int32_t y;
int32_t z;
} MpuVector32;
typedef struct MpuMotion6 {
MpuVector accel;
MpuVector gyro;
} MpuMotion6;
typedef struct MpuMotion32 {
MpuVector32 accel;
MpuVector32 gyro;
} MpuMotion32;
static mpu_t mpu6050;
static MpuMotion32 mpuAggrData;
static constexpr uint32_t mpuTickPeriodMs = 10;
static uint32_t mpuAggrCounter = 0;
static TimerHandle_t mpuTimerHandle;
static StaticTimer_t mpuTimerBuffer;
static uint16_t compressCoef = 4;
static void mpuTickCallback(TimerHandle_t tim) {
xEventGroupSetBits(i2cEventGroup, I2C_MPU_TICK);
}
static void mpuRead(MpuMotion6& data) {
mpu_getMotion6(&data.accel.x, &data.accel.y, &data.accel.z, &data.gyro.x,
&data.gyro.y, &data.gyro.z);
}
static void mpuSend(const MpuMotion32& data) {
// DEBUG(
// "DEBUG MPU SEND [%d] acc: x:%d; y:%d; z:%d | gyro: x:%d; y:%d; z:%d\n",
// mpuAggrCounter, data.accel.x, data.accel.y, data.accel.z, data.gyro.x,
// data.gyro.y, data.gyro.z);
CoprocStat status = {
.which_payload = CoprocStat_mpuStat_tag,
};
status.payload.mpuStat.accel.x = data.accel.x;
status.payload.mpuStat.accel.y = data.accel.y;
status.payload.mpuStat.accel.z = data.accel.z;
status.payload.mpuStat.has_accel = true;
status.payload.mpuStat.gyro.x = data.gyro.x;
status.payload.mpuStat.gyro.y = data.gyro.y;
status.payload.mpuStat.gyro.z = data.gyro.z;
status.payload.mpuStat.has_gyro = true;
status.payload.mpuStat.compressCoef = compressCoef;
dispatcherEnqueueStatus(status);
}
void mpuDispatch(const CoprocReq_MpuReq& req) {
switch (req.which_mpuCmd) {
case CoprocReq_MpuReq_init_tag:
mpuInitialize();
break;
case CoprocReq_MpuReq_oneSend_tag:
MpuMotion6 data;
MpuMotion32 data32;
mpuRead(data);
data32.accel.x = data.accel.x;
data32.accel.y = data.accel.y;
data32.accel.z = data.accel.z;
data32.gyro.x = data.gyro.x;
data32.gyro.y = data.gyro.y;
data32.gyro.z = data.gyro.z;
mpuSend(data32);
break;
case CoprocReq_MpuReq_startSend_tag: {
if (xTimerStart(mpuTimerHandle, 0) == pdFALSE) {
DEBUG("MPU6050 timer start failure\n");
}
mpuAggrCounter = 0;
break;
}
case CoprocReq_MpuReq_stopSend_tag: {
if (xTimerStop(mpuTimerHandle, 0) == pdFALSE) {
DEBUG("MPU6050 timer stop failure\n");
}
break;
}
case CoprocReq_MpuReq_setCompressCoef_tag: {
uint16_t coef = req.mpuCmd.setCompressCoef;
if (coef > 0 && coef <= 20) {
compressCoef = coef;
DEBUGLN("MPU6050 set new compression coef: %d", compressCoef);
}
break;
}
case CoprocReq_MpuReq_getCompressCoef_tag: {
CoprocStat status = {
.which_payload = CoprocStat_mpuStat_tag,
.payload = {
.mpuStat = {
.compressCoef = compressCoef,
},
},
};
dispatcherEnqueueStatus(status);
break;
}
};
}
void mpuTick() {
MpuMotion6 data;
mpuRead(data);
// DEBUG("MPU6050 [%d] acc: x:%d; y:%d; z:%d | gyro: x:%d; y:%d; z:%d\n",
// mpuAggrCounter, data.accel.x, data.accel.y, data.accel.z, data.gyro.x,
// data.gyro.y, data.gyro.z);
mpuAggrData.accel.x += data.accel.x;
mpuAggrData.accel.y += data.accel.y;
mpuAggrData.accel.z += data.accel.z;
mpuAggrData.gyro.x += data.gyro.x;
mpuAggrData.gyro.y += data.gyro.y;
mpuAggrData.gyro.z += data.gyro.z;
mpuAggrCounter++;
if (mpuAggrCounter >= compressCoef) {
mpuSend(mpuAggrData);
mpuAggrData.accel.x = 0;
mpuAggrData.accel.y = 0;
mpuAggrData.accel.z = 0;
mpuAggrData.gyro.x = 0;
mpuAggrData.gyro.y = 0;
mpuAggrData.gyro.z = 0;
mpuAggrCounter = 0;
}
}
void mpuCreate() {
mpu6050.devAddr = mpu_ADDRESS_AD0_HIGH;
mpuTimerHandle
= xTimerCreateStatic("mpuTimer", pdMS_TO_TICKS(mpuTickPeriodMs), true,
nullptr, mpuTickCallback, &mpuTimerBuffer);
}
void mpuInitialize() {
mpuReset();
if (mpu_testConnection()) {
mpu_setClockSource(mpu_CLOCK_PLL_XGYRO);
mpu_setFullScaleGyroRange(mpu_GYRO_FS_250);
mpu_setFullScaleAccelRange(mpu_ACCEL_FS_2);
mpu_setSleepEnabled(false);
} else {
DEBUG("MPU6050 is not connected\n");
CoprocStat status = {
.which_payload = CoprocStat_faultStat_tag,
.payload = {
.faultStat = {
.which_fault = CoprocStat_FaultStat_mpuFault_tag,
},
},
};
dispatcherEnqueueStatus(status);
}
}
void mpuReset() {
if (xTimerStop(mpuTimerHandle, 0) == pdFALSE) {
DEBUG("Time queue overflow\n");
}
}
bool mpu_testConnection() { return mpu_getDeviceID() == 0x34; }
// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
uint8_t mpu_getAuxVDDIOLevel() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_YG_OFFS_TC, mpu_TC_PWR_MODE_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setAuxVDDIOLevel(uint8_t level) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_YG_OFFS_TC, mpu_TC_PWR_MODE_BIT, level);
}
// SMPLRT_DIV register
uint8_t mpu_getRate() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_SMPLRT_DIV, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setRate(uint8_t rate) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_SMPLRT_DIV, rate);
}
// CONFIG register
uint8_t mpu_getExternalFrameSync() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_CONFIG, mpu_CFG_EXT_SYNC_SET_BIT,
mpu_CFG_EXT_SYNC_SET_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setExternalFrameSync(uint8_t sync) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_CONFIG, mpu_CFG_EXT_SYNC_SET_BIT,
mpu_CFG_EXT_SYNC_SET_LENGTH, sync);
}
uint8_t mpu_getDLPFMode() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_CONFIG, mpu_CFG_DLPF_CFG_BIT,
mpu_CFG_DLPF_CFG_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setDLPFMode(uint8_t mode) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_CONFIG, mpu_CFG_DLPF_CFG_BIT,
mpu_CFG_DLPF_CFG_LENGTH, mode);
}
// GYRO_CONFIG register
uint8_t mpu_getFullScaleGyroRange() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_GYRO_CONFIG, mpu_GCONFIG_FS_SEL_BIT,
mpu_GCONFIG_FS_SEL_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFullScaleGyroRange(uint8_t range) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_GYRO_CONFIG,
mpu_GCONFIG_FS_SEL_BIT, mpu_GCONFIG_FS_SEL_LENGTH, range);
}
// ACCEL_CONFIG register
bool mpu_getAccelXSelfTest() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_ACCEL_CONFIG, mpu_ACONFIG_XA_ST_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setAccelXSelfTest(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_ACCEL_CONFIG, mpu_ACONFIG_XA_ST_BIT, enabled);
}
bool mpu_getAccelYSelfTest() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_ACCEL_CONFIG, mpu_ACONFIG_YA_ST_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setAccelYSelfTest(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_ACCEL_CONFIG, mpu_ACONFIG_YA_ST_BIT, enabled);
}
bool mpu_getAccelZSelfTest() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_ACCEL_CONFIG, mpu_ACONFIG_ZA_ST_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setAccelZSelfTest(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_ACCEL_CONFIG, mpu_ACONFIG_ZA_ST_BIT, enabled);
}
uint8_t mpu_getFullScaleAccelRange() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_ACCEL_CONFIG,
mpu_ACONFIG_AFS_SEL_BIT, mpu_ACONFIG_AFS_SEL_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFullScaleAccelRange(uint8_t range) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_ACCEL_CONFIG,
mpu_ACONFIG_AFS_SEL_BIT, mpu_ACONFIG_AFS_SEL_LENGTH, range);
}
uint8_t mpu_getDHPFMode() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_ACCEL_CONFIG,
mpu_ACONFIG_ACCEL_HPF_BIT, mpu_ACONFIG_ACCEL_HPF_LENGTH, mpu6050.buffer,
0);
return mpu6050.buffer[0];
}
void mpu_setDHPFMode(uint8_t bandwidth) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_ACCEL_CONFIG,
mpu_ACONFIG_ACCEL_HPF_BIT, mpu_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
}
// FF_THR register
uint8_t mpu_getFreefallDetectionThreshold() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_FF_THR, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFreefallDetectionThreshold(uint8_t threshold) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_FF_THR, threshold);
}
// FF_DUR register
uint8_t mpu_getFreefallDetectionDuration() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_FF_DUR, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFreefallDetectionDuration(uint8_t duration) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_FF_DUR, duration);
}
// MOT_THR register
uint8_t mpu_getMotionDetectionThreshold() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_MOT_THR, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setMotionDetectionThreshold(uint8_t threshold) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_MOT_THR, threshold);
}
// MOT_DUR register
uint8_t mpu_getMotionDetectionDuration() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_MOT_DUR, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setMotionDetectionDuration(uint8_t duration) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_MOT_DUR, duration);
}
// ZRMOT_THR register
uint8_t mpu_getZeroMotionDetectionThreshold() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_ZRMOT_THR, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setZeroMotionDetectionThreshold(uint8_t threshold) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_ZRMOT_THR, threshold);
}
// ZRMOT_DUR register
uint8_t mpu_getZeroMotionDetectionDuration() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_ZRMOT_DUR, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setZeroMotionDetectionDuration(uint8_t duration) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_ZRMOT_DUR, duration);
}
// FIFO_EN register
bool mpu_getTempFIFOEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_TEMP_FIFO_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setTempFIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_TEMP_FIFO_EN_BIT, enabled);
}
bool mpu_getXGyroFIFOEnabled() {
I2Cdev_readBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_XG_FIFO_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setXGyroFIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_XG_FIFO_EN_BIT, enabled);
}
bool mpu_getYGyroFIFOEnabled() {
I2Cdev_readBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_YG_FIFO_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setYGyroFIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_YG_FIFO_EN_BIT, enabled);
}
bool mpu_getZGyroFIFOEnabled() {
I2Cdev_readBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_ZG_FIFO_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setZGyroFIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_ZG_FIFO_EN_BIT, enabled);
}
bool mpu_getAccelFIFOEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_ACCEL_FIFO_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setAccelFIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_ACCEL_FIFO_EN_BIT, enabled);
}
bool mpu_getSlave2FIFOEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_SLV2_FIFO_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave2FIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_SLV2_FIFO_EN_BIT, enabled);
}
bool mpu_getSlave1FIFOEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_SLV1_FIFO_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave1FIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_SLV1_FIFO_EN_BIT, enabled);
}
bool mpu_getSlave0FIFOEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_SLV0_FIFO_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave0FIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_FIFO_EN, mpu_SLV0_FIFO_EN_BIT, enabled);
}
// I2C_MST_CTRL register
bool mpu_getMultiMasterEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_MULT_MST_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setMultiMasterEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_MULT_MST_EN_BIT, enabled);
}
bool mpu_getWaitForExternalSensorEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_WAIT_FOR_ES_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setWaitForExternalSensorEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_WAIT_FOR_ES_BIT, enabled);
}
bool mpu_getSlave3FIFOEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_SLV_3_FIFO_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave3FIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_SLV_3_FIFO_EN_BIT, enabled);
}
bool mpu_getSlaveReadWriteTransitionEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_I2C_MST_P_NSR_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveReadWriteTransitionEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_I2C_MST_P_NSR_BIT, enabled);
}
uint8_t mpu_getMasterClockSpeed() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_I2C_MST_CLK_BIT,
mpu_I2C_MST_CLK_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setMasterClockSpeed(uint8_t speed) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_I2C_MST_CTRL, mpu_I2C_MST_CLK_BIT,
mpu_I2C_MST_CLK_LENGTH, speed);
}
// I2C_SLV* registers (Slave 0-3)
uint8_t mpu_getSlaveAddress(uint8_t num) {
if (num > 3)
return 0;
I2Cdev_readByte(
mpu6050.devAddr, mpu_RA_I2C_SLV0_ADDR + num * 3, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveAddress(uint8_t num, uint8_t address) {
if (num > 3)
return;
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_I2C_SLV0_ADDR + num * 3, address);
}
uint8_t mpu_getSlaveRegister(uint8_t num) {
if (num > 3)
return 0;
I2Cdev_readByte(
mpu6050.devAddr, mpu_RA_I2C_SLV0_REG + num * 3, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveRegister(uint8_t num, uint8_t reg) {
if (num > 3)
return;
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_I2C_SLV0_REG + num * 3, reg);
}
bool mpu_getSlaveEnabled(uint8_t num) {
if (num > 3)
return 0;
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveEnabled(uint8_t num, bool enabled) {
if (num > 3)
return;
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_EN_BIT, enabled);
}
bool mpu_getSlaveWordByteSwap(uint8_t num) {
if (num > 3)
return 0;
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_BYTE_SW_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveWordByteSwap(uint8_t num, bool enabled) {
if (num > 3)
return;
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_BYTE_SW_BIT, enabled);
}
bool mpu_getSlaveWriteMode(uint8_t num) {
if (num > 3)
return 0;
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_REG_DIS_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveWriteMode(uint8_t num, bool mode) {
if (num > 3)
return;
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_REG_DIS_BIT, mode);
}
bool mpu_getSlaveWordGroupOffset(uint8_t num) {
if (num > 3)
return 0;
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_GRP_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveWordGroupOffset(uint8_t num, bool enabled) {
if (num > 3)
return;
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_GRP_BIT, enabled);
}
uint8_t mpu_getSlaveDataLength(uint8_t num) {
if (num > 3)
return 0;
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_LEN_BIT, mpu_I2C_SLV_LEN_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveDataLength(uint8_t num, uint8_t length) {
if (num > 3)
return;
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_I2C_SLV0_CTRL + num * 3,
mpu_I2C_SLV_LEN_BIT, mpu_I2C_SLV_LEN_LENGTH, length);
}
// I2C_SLV* registers (Slave 4)
uint8_t mpu_getSlave4Address() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_I2C_SLV4_ADDR, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave4Address(uint8_t address) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_I2C_SLV4_ADDR, address);
}
uint8_t mpu_getSlave4Register() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_I2C_SLV4_REG, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave4Register(uint8_t reg) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_I2C_SLV4_REG, reg);
}
void mpu_setSlave4OutputByte(uint8_t data) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_I2C_SLV4_DO, data);
}
bool mpu_getSlave4Enabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL, mpu_I2C_SLV4_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave4Enabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL, mpu_I2C_SLV4_EN_BIT, enabled);
}
bool mpu_getSlave4InterruptEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL,
mpu_I2C_SLV4_INT_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave4InterruptEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL,
mpu_I2C_SLV4_INT_EN_BIT, enabled);
}
bool mpu_getSlave4WriteMode() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL,
mpu_I2C_SLV4_REG_DIS_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlave4WriteMode(bool mode) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL, mpu_I2C_SLV4_REG_DIS_BIT, mode);
}
uint8_t mpu_getSlave4MasterDelay() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL,
mpu_I2C_SLV4_MST_DLY_BIT, mpu_I2C_SLV4_MST_DLY_LENGTH, mpu6050.buffer,
0);
return mpu6050.buffer[0];
}
void mpu_setSlave4MasterDelay(uint8_t delay) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_I2C_SLV4_CTRL,
mpu_I2C_SLV4_MST_DLY_BIT, mpu_I2C_SLV4_MST_DLY_LENGTH, delay);
}
uint8_t mpu_getSlate4InputByte() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_I2C_SLV4_DI, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
// I2C_MST_STATUS register
bool mpu_getPassthroughStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_PASS_THROUGH_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getSlave4IsDone() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_I2C_SLV4_DONE_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getLostArbitration() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_I2C_LOST_ARB_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getSlave4Nack() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_I2C_SLV4_NACK_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getSlave3Nack() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_I2C_SLV3_NACK_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getSlave2Nack() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_I2C_SLV2_NACK_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getSlave1Nack() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_I2C_SLV1_NACK_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getSlave0Nack() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_STATUS,
mpu_MST_I2C_SLV0_NACK_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
// INT_PIN_CFG register
bool mpu_getInterruptMode() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_INT_LEVEL_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setInterruptMode(bool mode) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_INT_PIN_CFG, mpu_INTCFG_INT_LEVEL_BIT, mode);
}
bool mpu_getInterruptDrive() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG, mpu_INTCFG_INT_OPEN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setInterruptDrive(bool drive) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_INT_PIN_CFG, mpu_INTCFG_INT_OPEN_BIT, drive);
}
bool mpu_getInterruptLatch() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_LATCH_INT_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setInterruptLatch(bool latch) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_LATCH_INT_EN_BIT, latch);
}
bool mpu_getInterruptLatchClear() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_INT_RD_CLEAR_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setInterruptLatchClear(bool clear) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_INT_RD_CLEAR_BIT, clear);
}
bool mpu_getFSyncInterruptLevel() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_FSYNC_INT_LEVEL_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFSyncInterruptLevel(bool level) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_FSYNC_INT_LEVEL_BIT, level);
}
bool mpu_getFSyncInterruptEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_FSYNC_INT_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFSyncInterruptEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_FSYNC_INT_EN_BIT, enabled);
}
bool mpu_getI2CBypassEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_I2C_BYPASS_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setI2CBypassEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
bool mpu_getClockOutputEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_PIN_CFG,
mpu_INTCFG_CLKOUT_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setClockOutputEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_INT_PIN_CFG, mpu_INTCFG_CLKOUT_EN_BIT, enabled);
}
// INT_ENABLE register
uint8_t mpu_getIntEnabled() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntEnabled(uint8_t enabled) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_INT_ENABLE, enabled);
}
bool mpu_getIntFreefallEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu_INTERRUPT_FF_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntFreefallEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu_INTERRUPT_FF_BIT, enabled);
}
bool mpu_getIntMotionEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu_INTERRUPT_MOT_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntMotionEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu_INTERRUPT_MOT_BIT, enabled);
}
bool mpu_getIntZeroMotionEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu_INTERRUPT_ZMOT_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntZeroMotionEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu_INTERRUPT_ZMOT_BIT, enabled);
}
bool mpu_getIntFIFOBufferOverflowEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntFIFOBufferOverflowEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_FIFO_OFLOW_BIT, enabled);
}
bool mpu_getIntI2CMasterEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntI2CMasterEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_I2C_MST_INT_BIT, enabled);
}
bool mpu_getIntDataReadyEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntDataReadyEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_DATA_RDY_BIT, enabled);
}
// INT_STATUS register
uint8_t mpu_getIntStatus() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_INT_STATUS, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getIntFreefallStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS, mpu_INTERRUPT_FF_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getIntMotionStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS, mpu_INTERRUPT_MOT_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getIntZeroMotionStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS, mpu_INTERRUPT_ZMOT_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getIntFIFOBufferOverflowStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS,
mpu_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getIntI2CMasterStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS,
mpu_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getIntDataReadyStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS,
mpu_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
// ACCEL_*OUT_* registers
void mpu_getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx,
int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
mpu_getMotion6(ax, ay, az, gx, gy, gz);
// TODO: magnetometer integration
}
void mpu_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx,
int16_t* gy, int16_t* gz) {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_ACCEL_XOUT_H, 14, mpu6050.buffer, 0);
*ax = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
*ay = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
*az = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.buffer[5];
*gx = (((int16_t)mpu6050.buffer[8]) << 8) | mpu6050.buffer[9];
*gy = (((int16_t)mpu6050.buffer[10]) << 8) | mpu6050.buffer[11];
*gz = (((int16_t)mpu6050.buffer[12]) << 8) | mpu6050.buffer[13];
}
void mpu_getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
// void mpu_getAcceleration(int32_t* x, int32_t* y, int32_t* z) {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_ACCEL_XOUT_H, 6, mpu6050.buffer, 0);
*x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
*y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
*z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.buffer[5];
}
int16_t mpu_getAccelerationX() {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_ACCEL_XOUT_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
int16_t mpu_getAccelerationY() {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_ACCEL_YOUT_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
int16_t mpu_getAccelerationZ() {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_ACCEL_ZOUT_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
// TEMP_OUT_* registers
int16_t mpu_getTemperature() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_TEMP_OUT_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
// GYRO_*OUT_* registers
void mpu_getRotation(int16_t* x, int16_t* y, int16_t* z) {
// void mpu_getRotation(int32_t* x, int32_t* y, int32_t* z) {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_GYRO_XOUT_H, 6, mpu6050.buffer, 0);
*x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
*y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
*z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.buffer[5];
}
int16_t mpu_getRotationX() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_GYRO_XOUT_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
int16_t mpu_getRotationY() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_GYRO_YOUT_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
int16_t mpu_getRotationZ() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_GYRO_ZOUT_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
// EXT_SENS_DATA_* registers
uint8_t mpu_getExternalSensorByte(int position) {
I2Cdev_readByte(
mpu6050.devAddr, mpu_RA_EXT_SENS_DATA_00 + position, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
uint16_t mpu_getExternalSensorWord(int position) {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_EXT_SENS_DATA_00 + position, 2,
mpu6050.buffer, 0);
return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
uint32_t mpu_getExternalSensorDWord(int position) {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_EXT_SENS_DATA_00 + position, 4,
mpu6050.buffer, 0);
return (((uint32_t)mpu6050.buffer[0]) << 24)
| (((uint32_t)mpu6050.buffer[1]) << 16)
| (((uint16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
}
// MOT_DETECT_STATUS register
bool mpu_getXNegMotionDetected() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_MOT_DETECT_STATUS,
mpu_MOTION_MOT_XNEG_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getXPosMotionDetected() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_MOT_DETECT_STATUS,
mpu_MOTION_MOT_XPOS_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getYNegMotionDetected() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_MOT_DETECT_STATUS,
mpu_MOTION_MOT_YNEG_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getYPosMotionDetected() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_MOT_DETECT_STATUS,
mpu_MOTION_MOT_YPOS_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getZNegMotionDetected() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_MOT_DETECT_STATUS,
mpu_MOTION_MOT_ZNEG_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getZPosMotionDetected() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_MOT_DETECT_STATUS,
mpu_MOTION_MOT_ZPOS_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getZeroMotionDetected() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_MOT_DETECT_STATUS,
mpu_MOTION_MOT_ZRMOT_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
// I2C_SLV*_DO register
void mpu_setSlaveOutputByte(uint8_t num, uint8_t data) {
if (num > 3)
return;
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_I2C_SLV0_DO + num, data);
}
// I2C_MST_DELAY_CTRL register
bool mpu_getExternalShadowDelayEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_I2C_MST_DELAY_CTRL,
mpu_DELAYCTRL_DELAY_ES_SHADOW_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setExternalShadowDelayEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_I2C_MST_DELAY_CTRL,
mpu_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
}
bool mpu_getSlaveDelayEnabled(uint8_t num) {
// mpu_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
if (num > 4)
return 0;
I2Cdev_readBit(
mpu6050.devAddr, mpu_RA_I2C_MST_DELAY_CTRL, num, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSlaveDelayEnabled(uint8_t num, bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_I2C_MST_DELAY_CTRL, num, enabled);
}
// SIGNAL_PATH_RESET register
void mpu_resetGyroscopePath() {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_SIGNAL_PATH_RESET,
mpu_PATHRESET_GYRO_RESET_BIT, true);
}
void mpu_resetAccelerometerPath() {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_SIGNAL_PATH_RESET,
mpu_PATHRESET_ACCEL_RESET_BIT, true);
}
void mpu_resetTemperaturePath() {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_SIGNAL_PATH_RESET,
mpu_PATHRESET_TEMP_RESET_BIT, true);
}
// MOT_DETECT_CTRL register
uint8_t mpu_getAccelerometerPowerOnDelay() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_MOT_DETECT_CTRL,
mpu_DETECT_ACCEL_ON_DELAY_BIT, mpu_DETECT_ACCEL_ON_DELAY_LENGTH,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setAccelerometerPowerOnDelay(uint8_t delay) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_MOT_DETECT_CTRL,
mpu_DETECT_ACCEL_ON_DELAY_BIT, mpu_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
}
uint8_t mpu_getFreefallDetectionCounterDecrement() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_MOT_DETECT_CTRL,
mpu_DETECT_FF_COUNT_BIT, mpu_DETECT_FF_COUNT_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFreefallDetectionCounterDecrement(uint8_t decrement) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_MOT_DETECT_CTRL,
mpu_DETECT_FF_COUNT_BIT, mpu_DETECT_FF_COUNT_LENGTH, decrement);
}
uint8_t mpu_getMotionDetectionCounterDecrement() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_MOT_DETECT_CTRL,
mpu_DETECT_MOT_COUNT_BIT, mpu_DETECT_MOT_COUNT_LENGTH, mpu6050.buffer,
0);
return mpu6050.buffer[0];
}
void mpu_setMotionDetectionCounterDecrement(uint8_t decrement) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_MOT_DETECT_CTRL,
mpu_DETECT_MOT_COUNT_BIT, mpu_DETECT_MOT_COUNT_LENGTH, decrement);
}
// USER_CTRL register
bool mpu_getFIFOEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_USER_CTRL, mpu_USERCTRL_FIFO_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setFIFOEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_USER_CTRL, mpu_USERCTRL_FIFO_EN_BIT, enabled);
}
bool mpu_getI2CMasterModeEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_USER_CTRL,
mpu_USERCTRL_I2C_MST_EN_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setI2CMasterModeEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_USER_CTRL,
mpu_USERCTRL_I2C_MST_EN_BIT, enabled);
}
void mpu_switchSPIEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_USER_CTRL,
mpu_USERCTRL_I2C_IF_DIS_BIT, enabled);
}
void mpu_resetFIFO() {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_USER_CTRL, mpu_USERCTRL_FIFO_RESET_BIT, true);
}
void mpu_resetI2CMaster() {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_USER_CTRL,
mpu_USERCTRL_I2C_MST_RESET_BIT, true);
}
void mpu_resetSensors() {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_USER_CTRL,
mpu_USERCTRL_SIG_COND_RESET_BIT, true);
}
// PWR_MGMT_1 register
void mpu_reset() {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_DEVICE_RESET_BIT, true);
}
bool mpu_getSleepEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_SLEEP_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setSleepEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_SLEEP_BIT, enabled);
}
bool mpu_getWakeCycleEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_CYCLE_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setWakeCycleEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_CYCLE_BIT, enabled);
}
bool mpu_getTempSensorEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_TEMP_DIS_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0] == 0; // 1 is actually disabled here
}
void mpu_setTempSensorEnabled(bool enabled) {
// 1 is actually disabled here
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_TEMP_DIS_BIT, !enabled);
}
uint8_t mpu_getClockSource() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_CLKSEL_BIT,
mpu_PWR1_CLKSEL_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setClockSource(uint8_t source) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_PWR_MGMT_1, mpu_PWR1_CLKSEL_BIT,
mpu_PWR1_CLKSEL_LENGTH, source);
}
// PWR_MGMT_2 register
uint8_t mpu_getWakeFrequency() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_PWR_MGMT_2,
mpu_PWR2_LP_WAKE_CTRL_BIT, mpu_PWR2_LP_WAKE_CTRL_LENGTH, mpu6050.buffer,
0);
return mpu6050.buffer[0];
}
void mpu_setWakeFrequency(uint8_t frequency) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_PWR_MGMT_2,
mpu_PWR2_LP_WAKE_CTRL_BIT, mpu_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
}
bool mpu_getStandbyXAccelEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_XA_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setStandbyXAccelEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_XA_BIT, enabled);
}
bool mpu_getStandbyYAccelEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_YA_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setStandbyYAccelEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_YA_BIT, enabled);
}
bool mpu_getStandbyZAccelEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_ZA_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setStandbyZAccelEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_ZA_BIT, enabled);
}
bool mpu_getStandbyXGyroEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_XG_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setStandbyXGyroEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_XG_BIT, enabled);
}
bool mpu_getStandbyYGyroEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_YG_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setStandbyYGyroEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_YG_BIT, enabled);
}
bool mpu_getStandbyZGyroEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_ZG_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setStandbyZGyroEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_PWR_MGMT_2, mpu_PWR2_STBY_ZG_BIT, enabled);
}
// FIFO_COUNT* registers
uint16_t mpu_getFIFOCount() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_FIFO_COUNTH, 2, mpu6050.buffer, 0);
return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
// FIFO_R_W register
uint8_t mpu_getFIFOByte() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_FIFO_R_W, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_getFIFOBytes(uint8_t* data, uint8_t length) {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_FIFO_R_W, length, data, 0);
}
void mpu_setFIFOByte(uint8_t data) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_FIFO_R_W, data);
}
// WHO_AM_I register
uint8_t mpu_getDeviceID() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_WHO_AM_I, mpu_WHO_AM_I_BIT,
mpu_WHO_AM_I_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setDeviceID(uint8_t id) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_WHO_AM_I, mpu_WHO_AM_I_BIT,
mpu_WHO_AM_I_LENGTH, id);
}
// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
// XG_OFFS_TC register
uint8_t mpu_getOTPBankValid() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_XG_OFFS_TC, mpu_TC_OTP_BNK_VLD_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setOTPBankValid(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_XG_OFFS_TC, mpu_TC_OTP_BNK_VLD_BIT, enabled);
}
int8_t mpu_getXGyroOffsetTC() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_XG_OFFS_TC, mpu_TC_OFFSET_BIT,
mpu_TC_OFFSET_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setXGyroOffsetTC(int8_t offset) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_XG_OFFS_TC, mpu_TC_OFFSET_BIT,
mpu_TC_OFFSET_LENGTH, offset);
}
// YG_OFFS_TC register
int8_t mpu_getYGyroOffsetTC() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_YG_OFFS_TC, mpu_TC_OFFSET_BIT,
mpu_TC_OFFSET_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setYGyroOffsetTC(int8_t offset) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_YG_OFFS_TC, mpu_TC_OFFSET_BIT,
mpu_TC_OFFSET_LENGTH, offset);
}
// ZG_OFFS_TC register
int8_t mpu_getZGyroOffsetTC() {
I2Cdev_readBits(mpu6050.devAddr, mpu_RA_ZG_OFFS_TC, mpu_TC_OFFSET_BIT,
mpu_TC_OFFSET_LENGTH, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setZGyroOffsetTC(int8_t offset) {
I2Cdev_writeBits(mpu6050.devAddr, mpu_RA_ZG_OFFS_TC, mpu_TC_OFFSET_BIT,
mpu_TC_OFFSET_LENGTH, offset);
}
// X_FINE_GAIN register
int8_t mpu_getXFineGain() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_X_FINE_GAIN, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setXFineGain(int8_t gain) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_X_FINE_GAIN, gain);
}
// Y_FINE_GAIN register
int8_t mpu_getYFineGain() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_Y_FINE_GAIN, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setYFineGain(int8_t gain) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_Y_FINE_GAIN, gain);
}
// Z_FINE_GAIN register
int8_t mpu_getZFineGain() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_Z_FINE_GAIN, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setZFineGain(int8_t gain) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_Z_FINE_GAIN, gain);
}
// XA_OFFS_* registers
int16_t mpu_getXAccelOffset() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_XA_OFFS_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
void mpu_setXAccelOffset(int16_t offset) {
I2Cdev_writeWord(mpu6050.devAddr, mpu_RA_XA_OFFS_H, offset);
}
// YA_OFFS_* register
int16_t mpu_getYAccelOffset() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_YA_OFFS_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
void mpu_setYAccelOffset(int16_t offset) {
I2Cdev_writeWord(mpu6050.devAddr, mpu_RA_YA_OFFS_H, offset);
}
// ZA_OFFS_* register
int16_t mpu_getZAccelOffset() {
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_ZA_OFFS_H, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
void mpu_setZAccelOffset(int16_t offset) {
I2Cdev_writeWord(mpu6050.devAddr, mpu_RA_ZA_OFFS_H, offset);
}
// XG_OFFS_USR* registers
int16_t mpu_getXGyroOffset() {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_XG_OFFS_USRH, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
void mpu_setXGyroOffset(int16_t offset) {
I2Cdev_writeWord(mpu6050.devAddr, mpu_RA_XG_OFFS_USRH, offset);
}
// YG_OFFS_USR* register
int16_t mpu_getYGyroOffset() {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_YG_OFFS_USRH, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
void mpu_setYGyroOffset(int16_t offset) {
I2Cdev_writeWord(mpu6050.devAddr, mpu_RA_YG_OFFS_USRH, offset);
}
// ZG_OFFS_USR* register
int16_t mpu_getZGyroOffset() {
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_ZG_OFFS_USRH, 2, mpu6050.buffer, 0);
return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
}
void mpu_setZGyroOffset(int16_t offset) {
I2Cdev_writeWord(mpu6050.devAddr, mpu_RA_ZG_OFFS_USRH, offset);
}
// INT_ENABLE register (DMP functions)
bool mpu_getIntPLLReadyEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntPLLReadyEnabled(bool enabled) {
I2Cdev_writeBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_PLL_RDY_INT_BIT, enabled);
}
bool mpu_getIntDMPEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_ENABLE,
mpu_INTERRUPT_DMP_INT_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setIntDMPEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_INT_ENABLE, mpu_INTERRUPT_DMP_INT_BIT, enabled);
}
// DMP_INT_STATUS
bool mpu_getDMPInt5Status() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_DMP_INT_STATUS, mpu_DMPINT_5_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getDMPInt4Status() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_DMP_INT_STATUS, mpu_DMPINT_4_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getDMPInt3Status() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_DMP_INT_STATUS, mpu_DMPINT_3_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getDMPInt2Status() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_DMP_INT_STATUS, mpu_DMPINT_2_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getDMPInt1Status() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_DMP_INT_STATUS, mpu_DMPINT_1_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getDMPInt0Status() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_DMP_INT_STATUS, mpu_DMPINT_0_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
// INT_STATUS register (DMP functions)
bool mpu_getIntPLLReadyStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS,
mpu_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
bool mpu_getIntDMPStatus() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_INT_STATUS,
mpu_INTERRUPT_DMP_INT_BIT, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
// USER_CTRL register (DMP functions)
bool mpu_getDMPEnabled() {
I2Cdev_readBit(mpu6050.devAddr, mpu_RA_USER_CTRL, mpu_USERCTRL_DMP_EN_BIT,
mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setDMPEnabled(bool enabled) {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_USER_CTRL, mpu_USERCTRL_DMP_EN_BIT, enabled);
}
void mpu_resetDMP() {
I2Cdev_writeBit(
mpu6050.devAddr, mpu_RA_USER_CTRL, mpu_USERCTRL_DMP_RESET_BIT, true);
}
// BANK_SEL register
void mpu_setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
bank &= 0x1F;
if (userBank)
bank |= 0x20;
if (prefetchEnabled)
bank |= 0x40;
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_BANK_SEL, bank);
}
// MEM_START_ADDR register
void mpu_setMemoryStartAddress(uint8_t address) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_MEM_START_ADDR, address);
}
// MEM_R_W register
uint8_t mpu_readMemoryByte() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_MEM_R_W, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_writeMemoryByte(uint8_t data) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_MEM_R_W, data);
}
void mpu_readMemoryBlock(
uint8_t* data, uint16_t dataSize, uint8_t bank, uint8_t address) {
mpu_setMemoryBank(bank, false, false);
mpu_setMemoryStartAddress(address);
uint8_t chunkSize;
for (uint16_t i = 0; i < dataSize;) {
// determine correct chunk size according to bank position and data size
chunkSize = mpu_DMP_MEMORY_CHUNK_SIZE;
// make sure we don't go past the data size
if (i + chunkSize > dataSize)
chunkSize = dataSize - i;
// make sure this chunk doesn't go past the bank boundary (256 bytes)
if (chunkSize > 256 - address)
chunkSize = 256 - address;
// read the chunk of data as specified
I2Cdev_readBytes(
mpu6050.devAddr, mpu_RA_MEM_R_W, chunkSize, data + i, 0);
// increase byte index by [chunkSize]
i += chunkSize;
// uint8_t automatically wraps to 0 at 256
address += chunkSize;
// if we aren't done, update bank (if necessary) and address
if (i < dataSize) {
if (address == 0)
bank++;
mpu_setMemoryBank(bank, false, false);
mpu_setMemoryStartAddress(address);
}
}
}
/*bool mpu_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
mpu_setMemoryBank(bank, false, false);
mpu_setMemoryStartAddress(address);
uint8_t chunkSize;
uint8_t *verifyBuffer;
uint8_t *progBuffer;
uint16_t i;
uint8_t j;
if (verify) verifyBuffer = (uint8_t *)malloc(mpu_DMP_MEMORY_CHUNK_SIZE);
if (useProgMem) progBuffer = (uint8_t *)malloc(mpu_DMP_MEMORY_CHUNK_SIZE);
for (i = 0; i < dataSize;) {
// determine correct chunk size according to bank position and data size
chunkSize = mpu_DMP_MEMORY_CHUNK_SIZE;
// make sure we don't go past the data size
if (i + chunkSize > dataSize) chunkSize = dataSize - i;
// make sure this chunk doesn't go past the bank boundary (256 bytes)
if (chunkSize > 256 - address) chunkSize = 256 - address;
if (useProgMem) {
// write the chunk of data as specified
for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
} else {
// write the chunk of data as specified
progBuffer = (uint8_t *)data + i;
}
I2Cdev_writeBytes(mpu6050.devAddr, mpu_RA_MEM_R_W, chunkSize, progBuffer);
// verify data if needed
if (verify && verifyBuffer) {
mpu_setMemoryBank(bank, false, false);
mpu_setMemoryStartAddress(address);
I2Cdev_readBytes(mpu6050.devAddr, mpu_RA_MEM_R_W, chunkSize, verifyBuffer);
if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
//Serial.print("Block write verification error, bank ");
//Serial.print(bank, DEC);
//Serial.print(", address ");
//Serial.print(address, DEC);
//Serial.print("!\nExpected:");
//for (j = 0; j < chunkSize; j++) {
// Serial.print(" 0x");
// if (progBuffer[j] < 16) Serial.print("0");
// Serial.print(progBuffer[j], HEX);
//}
//Serial.print("\nReceived:");
//for (uint8_t j = 0; j < chunkSize; j++) {
// Serial.print(" 0x");
// if (verifyBuffer[i + j] < 16) Serial.print("0");
// Serial.print(verifyBuffer[i + j], HEX);
//}
Serial.print("\n");
free(verifyBuffer);
if (useProgMem) free(progBuffer);
return false; // uh oh.
}
}
// increase byte index by [chunkSize]
i += chunkSize;
// uint8_t automatically wraps to 0 at 256
address += chunkSize;
// if we aren't done, update bank (if necessary) and address
if (i < dataSize) {
if (address == 0) bank++;
mpu_setMemoryBank(bank, false, false);
mpu_setMemoryStartAddress(address);
}
}
if (verify) free(verifyBuffer);
if (useProgMem) free(progBuffer);
return true;
}
bool mpu_writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
return mpu_writeMemoryBlock(data, dataSize, bank, address, verify, true);
}
bool mpu_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
uint8_t *progBuffer, success, special;
uint16_t i, j;
if (useProgMem) {
progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary
}
// config set data is a long string of blocks with the following structure:
// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
uint8_t bank, offset, length;
for (i = 0; i < dataSize;) {
if (useProgMem) {
bank = pgm_read_byte(data + i++);
offset = pgm_read_byte(data + i++);
length = pgm_read_byte(data + i++);
} else {
bank = data[i++];
offset = data[i++];
length = data[i++];
}
// write data or perform special action
if (length > 0) {
// regular block of data to write
//Serial.print("Writing config block to bank ");
//Serial.print(bank);
//Serial.print(", offset ");
//Serial.print(offset);
//Serial.print(", length=");
//Serial.println(length);
if (useProgMem) {
if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length);
for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j);
} else {
progBuffer = (uint8_t *)data + i;
}
success = mpu_writeMemoryBlock(progBuffer, length, bank, offset, true);
i += length;
} else {
// special instruction
// NOTE: this kind of behavior (what and when to do certain things)
// is totally undocumented. This code is in here based on observed
// behavior only, and exactly why (or even whether) it has to be here
// is anybody's guess for now.
if (useProgMem) {
special = pgm_read_byte(data + i++);
} else {
special = data[i++];
}
//Serial.print("Special command code ");
//Serial.print(special, HEX);
//Serial.println(" found...");
if (special == 0x01) {
// enable DMP-related interrupts
//setIntZeroMotionEnabled(true);
//setIntFIFOBufferOverflowEnabled(true);
//setIntDMPEnabled(true);
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_INT_ENABLE, 0x32); // single operation
success = true;
} else {
// unknown special command
success = false;
}
}
if (!success) {
if (useProgMem) free(progBuffer);
return false; // uh oh
}
}
if (useProgMem) free(progBuffer);
return true;
}
bool mpu_writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
return mpu_writeDMPConfigurationSet(data, dataSize, true);
}*/
// DMP_CFG_1 register
uint8_t mpu_getDMPConfig1() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_DMP_CFG_1, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setDMPConfig1(uint8_t config) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_DMP_CFG_1, config);
}
// DMP_CFG_2 register
uint8_t mpu_getDMPConfig2() {
I2Cdev_readByte(mpu6050.devAddr, mpu_RA_DMP_CFG_2, mpu6050.buffer, 0);
return mpu6050.buffer[0];
}
void mpu_setDMPConfig2(uint8_t config) {
I2Cdev_writeByte(mpu6050.devAddr, mpu_RA_DMP_CFG_2, config);
}