|
|
本帖最后由 弘毅 于 2013-5-22 22:10 编辑
这个是MPU6050.CPP核心代码翻译的第三部分，也是最终部分。。从2005行开始~~
如转载本帖需获得作者许可。
[pre lang="arduino" line="2005"]/** 重置FIFO
* 当设置为1时，此位将重置FIFO缓冲区，此时FIFO_EN等于0。触发重置后，此位将自动清为0.
* @请参见MPU6050_RA_USER_CTRL字段
* @请参见MPU6050_USERCTRL_FIFO_RESET_BIT字段
*/
void MPU6050::resetFIFO() {
I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true);
}
/** 重置I2C主机
* 当设置为1时，此位将重置I2C主机，此时I2C_MST_EN等于0。触发重置后，此位将自动清为0.
* @请参见MPU6050_RA_USER_CTRL字段
* @请参见MPU6050_USERCTRL_I2C_MST_RESET_BIT字段
*/
void MPU6050::resetI2CMaster() {
I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
}
/**重置所有传感器寄存器和信号路径。
* 当设置为1时，此位将重置所有传感器寄存器的信号路径（陀螺仪、加速度传感器和温度传感器）。这一操作也会清除传感器寄存器。触发重置后，此位将自动清为0.
*
* 如果只重置信号路径（不重置传感器寄存器），请使用寄存器104，即SIGNAL_PATH_RESET。
*
* @请参见MPU6050_RA_USER_CTRL字段
*@请参见MPU6050_USERCTRL_SIG_COND_RESET_BIT字段
*/
void MPU6050::resetSensors() {
I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
}
// PWR_MGMT_1寄存器
/** 触发一个设备的完整重置。
* 触发重置后，一个~ 50 毫秒的小延迟是合理的。
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_DEVICE_RESET_BIT字段
*/
void MPU6050::reset() {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
}
/** 获取休眠模式状态
* 寄存器的SLEEP位设置使设备处于非常低功率的休眠模式。在这种模式下,只有串行接口和内部寄存器处于运行状态,因此只有非常低的待机电流。清除此位，设备将回到正常模式。为了节电，如果陀螺仪的任何轴都不被应用程序使用，那么每个陀螺仪的个人备用选择就可用。
* @返回当前休眠模式启用状态
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_SLEEP_BIT字段
*/
bool MPU6050::getSleepEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);
return buffer[0];
}
/** 设定休眠模式状态
* @新休眠模式状态启用参数
* @请参见getSleepEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_SLEEP_BIT字段
*/
void MPU6050::setSleepEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/** 获取唤醒周期启用状态
* 当此位设为1且SLEEP禁用时，MPU-60X0会以LP_WAKE_CTRL (寄存器 108)决定的速率在休眠模式和唤醒模式间循环，以此从活跃的传感器中获取数据样本。
* @返回当前休眠模式启用状态
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_CYCLE_BIT字段
*/
bool MPU6050::getWakeCycleEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer);
return buffer[0];
}
/** 设定唤醒周期启用状态
* @新休眠模式状态启用参数
* @请参见getWakeCycleEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_CYCLE_BIT字段
*/
void MPU6050::setWakeCycleEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
}
/** 获取温度传感器启用状态
* 控制内部温度传感器的使用
*
* 注意:这个寄存器存储* disabled *值,但为了它与其余代码的一致性,函数以真/假值来命名和使用，以此来表明传感器是否是启用或禁用。
*
* @返回当前温度传感器启用状态
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_TEMP_DIS_BIT字段
*/
bool MPU6050::getTempSensorEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);
return buffer[0] == 0; // 1 is actually disabled here
}
/** 设定温度传感器启用状态 控制内部温度传感器的使用
* 意:这个寄存器存储* disabled *值,但为了它与其余代码的一致性,函数以真/假值来命名和使用，以此来表明传感器是否是启用或禁用。
*
* @新温度传感器启用参数
* @请参见getTempSensorEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_TEMP_DIS_BIT字段
*/
void MPU6050::setTempSensorEnabled(bool enabled) {
// 1 is actually disabled here
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
}
/** 获取时钟源设置
* @返回当前时钟源设置
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_CLKSEL_BIT字段
* @请参见MPU6050_PWR1_CLKSEL_LENGTH字段
*/
uint8_t MPU6050::getClockSource() {
I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer);
return buffer[0];
}
/** 设定时钟源设置
* 一个频率为8 mhz的内部振荡器,基于陀螺仪的时钟或外部信息源都可以被选为MPU-60X0的时钟源。当频率为8 mhz的内部振荡器或外部信息源被选为时钟源时,MPU-60X0可以在低功率模式且禁用陀螺仪的情况下运行。
*
* 在启用时,MPU-60X0时钟源默认为内部振荡器。但是,为了更好的稳定性，强烈建议使用陀螺仪（或外部时钟源）来作为参考。可根据下表选择时钟源：
*
* <pre>
* CLK_SEL | 时钟源
* --------+--------------------------------------
* 0 | 内部振荡器
* 1 | PLL with X Gyro reference
* 2 | PLL with Y Gyro reference
* 3 | PLL with Z Gyro reference
* 4 | PLL with external 32.768kHz reference
* 5 | PLL with external 19.2MHz reference
* 6 | Reserved
* 7 | Stops the clock and keeps the timing generator in reset
* </pre>
*
* @新时钟源设置参数
* @请参见getClockSource()字段
* @请参见MPU6050_RA_PWR_MGMT_1字段
* @请参见MPU6050_PWR1_CLKSEL_BIT字段
* @请参见MPU6050_PWR1_CLKSEL_LENGTH字段
*/
void MPU6050::setClockSource(uint8_t source) {
I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}
// PWR_MGMT_2寄存器
/** 获取Accel-Only低电量模式下的唤醒频率
* 通过把Power Management 1寄存器（寄存器107）中的PWRSEL设为1，MPU-60X0可以处于Accerlerometer Only的低电量模式。在这种模式下,设备将关闭除了原I2C接口以外的所有设备，只留下accelerometer以固定时间间隔醒来进行测量。唤醒频率可用LP_WAKE_CTRL进行配置，如下表所示：
*
* <pre>
* LP_WAKE_CTRL | 唤醒频率
* -------------+------------------
* 0 | 1.25 Hz
* 1 | 2.5 Hz
* 2 | 5 Hz
* 3 | 10 Hz
* <pre>
*
* 更多MPU-60X0电量模式的相关信息，请参阅寄存器107.
*
* @返回当前唤醒频率
* @请参见MPU6050_RA_PWR_MGMT_2字段
*/
uint8_t MPU6050::getWakeFrequency() {
I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
return buffer[0];
}
/** 设置Accel-Only低电量模式下的唤醒频率
* @新唤醒频率参数
* @请参见MPU6050_RA_PWR_MGMT_2字段
*/
void MPU6050::setWakeFrequency(uint8_t frequency) {
I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
}
/** 获取备用的x轴加速度传感器启用状态
* 如果启用该传感器，x轴不会收集或报告信息（或耗电）
* @返回当前备用的x轴启用状态
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_XA_BIT字段
*/
bool MPU6050::getStandbyXAccelEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer);
return buffer[0];
}
/** 设定备用的x轴加速度传感器启用状态
*@新的备用x轴启用状态
* @请参见getStandbyXAccelEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_XA_BIT字段
*/
void MPU6050::setStandbyXAccelEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
}
/** 获取备用的Y轴加速度传感器启用状态
* 如果启用该传感器，Y轴不会收集或报告信息（或耗电）
* @返回当前备用的Y轴启用状态
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_YA_BIT字段
*/
bool MPU6050::getStandbyYAccelEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer);
return buffer[0];
}
/** 设定备用的Y轴加速度传感器启用状态
* @新的备用Y轴启用状态
* @请参见getStandbyYAccelEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_YA_BIT字段
*/
void MPU6050::setStandbyYAccelEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled);
}
/** 获取备用的Z轴加速度传感器启用状态
* 如果启用该传感器，Z轴不会收集或报告信息（或耗电）
* @返回当前备用的Z轴启用状态
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_ZA_BIT字段
*/
bool MPU6050::getStandbyZAccelEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer);
return buffer[0];
}
/** 设定备用的Z轴加速度传感器启用状态
* @新的备用Z轴启用状态
* @请参见getStandbyZAccelEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_ZA_BIT字段
*/
void MPU6050::setStandbyZAccelEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled);
}
/** 获取备用的x轴陀螺仪启用状态
* 如果启用该传感器，x轴不会收集或报告信息（或耗电）
* @返回当前备用的x轴启用状态
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_XG_BIT字段
*/
bool MPU6050::getStandbyXGyroEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer);
return buffer[0];
}
/** 设定备用的x轴陀螺仪启用状态
* @新的备用x轴启用状态
* @请参见getStandbyXGyroEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_XG_BIT字段
*/
void MPU6050::setStandbyXGyroEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled);
}
/** 获取备用的Y轴陀螺仪启用状态
* 如果启用该传感器，Y轴不会收集或报告信息（或耗电）
* @返回当前备用的Y轴启用状态
* @@请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_YG_BIT字段
*/
bool MPU6050::getStandbyYGyroEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer);
return buffer[0];
}
/** 设定备用的Y轴陀螺仪启用状态
* @新的备用Y轴启用状态
* @请参见getStandbyYGyroEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_YG_BIT字段
*/
void MPU6050::setStandbyYGyroEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled);
}
/**获取备用的Z轴陀螺仪启用状态
* 如果启用该传感器，Z轴不会收集或报告信息（或耗电）
* @返回当前备用的Z轴启用状态
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_ZG_BIT字段
*/
bool MPU6050::getStandbyZGyroEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer);
return buffer[0];
}
/** 设定备用的Z轴陀螺仪启用状态
* @新的备用Z轴启用状态
* @请参见getStandbyZGyroEnabled()字段
* @请参见MPU6050_RA_PWR_MGMT_2字段
* @请参见MPU6050_PWR2_STBY_ZG_BIT字段
*/
void MPU6050::setStandbyZGyroEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
}
// FIFO_COUNT*寄存器
/**获取当前FIFO缓冲区大小
* 这个值表明了存储于FIFO缓冲区的字节数。而这个数字也是能从FIFO缓冲区读取的字节数，它与存储在FIFO(寄存器35和36)中的传感器数据组所提供的可用样本数成正比。
* @返回当前FIFO缓冲区大小
*/
uint16_t MPU6050::getFIFOCount() {
I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
return (((uint16_t)buffer[0]) << 8) | buffer[1];
}
// FIFO_R_W存储器
/**从FIFO缓冲区中获取字节
* 这个寄存器用于从FIFO缓冲区中读取和编写数据。数据在寄存器编号(从低到高)的指令下编写入数据写入FIFO。如果所有的FIFO启用标志(见下文)都被启用了且所有外部传感器数据寄存器(寄存器73至寄存器96)都与一个slave设备相连,那么寄存器59到寄存器96的内容都将在采样率的指令下编写。
*
* 当传感器数据寄存器（寄存器59到寄存器96）的相关FIFO启用标志在FIFO_EN 寄存器35)中都设为1时，它们的内容将被写入FIFO缓冲区。在I2C_MST_CTRL (寄存器 36)中能找到一个与I2C Slave 3相连的额外的传感器数据寄存器标志。
*
* 如果FIFO缓冲区溢出,状态位FIFO_OFLOW_INT自动设置为1。此位位于INT_STATUS (寄存器58)中。当FIFO缓冲区溢出时,最早的数据将会丢失而新数据将被写入FIFO。
*
* 如果FIFO缓冲区为空, 读取将返回原来从FIFO中读取的最后一个字节，直到有可用的新数据。用户应检查FIFO_COUNT,以确保不在FIFO缓冲为空时读取。
*
* @return Byte from FIFO buffer
*/
uint8_t MPU6050::getFIFOByte() {
I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer);
return buffer[0];
}
void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
}
/** 在FIFO缓冲区编写字节
*@请参见getFIFOByte()字段
*@请参见MPU6050_RA_FIFO_R_W字段
*/
void MPU6050::setFIFOByte(uint8_t data) {
I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
}
//WHO_AM_I寄存器
/**获取设备ID
* 这个寄存器是用来验证设备的身份的 (0,0 b110100 x34)。
* @返回设备ID (只限6位! 应为 0x34)
* @请参见MPU6050_RA_WHO_AM_I字段
* @请参见MPU6050_WHO_AM_I_BIT字段
* @请参见MPU6050_WHO_AM_I_LENGTH字段
*/
uint8_t MPU6050::getDeviceID() {
I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
return buffer[0];
}
/** 设定设备ID
* 在WHO_AM_I寄存器中写入一个新的ID（仍不知道进行这一步的原因）
* @设置新设备的ID参数.
* @请参见getDeviceID()字段
* @请参见MPU6050_RA_WHO_AM_I字段
* @请参见MPU6050_WHO_AM_I_BIT字段
* @请参见MPU6050_WHO_AM_I_LENGTH字段
*/
void MPU6050::setDeviceID(uint8_t id) {
I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id);
}
// ========未文档化的/DMP寄存器/方法 ========
// XG_OFFS_TC寄存器
uint8_t MPU6050::getOTPBankValid() {
I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
return buffer[0];
}
void MPU6050::setOTPBankValid(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
}
int8_t MPU6050::getXGyroOffset() {
I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void MPU6050::setXGyroOffset(int8_t offset) {
I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
}
//YG_OFFS_TC寄存器
int8_t MPU6050::getYGyroOffset() {
I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void MPU6050::setYGyroOffset(int8_t offset) {
I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
}
// ZG_OFFS_TC寄存器
int8_t MPU6050::getZGyroOffset() {
I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void MPU6050::setZGyroOffset(int8_t offset) {
I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
}
// X_FINE_GAIN寄存器
int8_t MPU6050::getXFineGain() {
I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
return buffer[0];
}
void MPU6050::setXFineGain(int8_t gain) {
I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
}
// Y_FINE_GAIN寄存器
int8_t MPU6050::getYFineGain() {
I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
return buffer[0];
}
void MPU6050::setYFineGain(int8_t gain) {
I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
}
// Z_FINE_GAIN寄存器
int8_t MPU6050::getZFineGain() {
I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
return buffer[0];
}
void MPU6050::setZFineGain(int8_t gain) {
I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
}
//XA_OFFS_*寄存器
int16_t MPU6050::getXAccelOffset() {
I2Cdev::readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setXAccelOffset(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU6050_RA_XA_OFFS_H, offset);
}
//YA_OFFS_*寄存器
int16_t MPU6050::getYAccelOffset() {
I2Cdev::readBytes(devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setYAccelOffset(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU6050_RA_YA_OFFS_H, offset);
}
// ZA_OFFS_*寄存器
int16_t MPU6050::getZAccelOffset() {
I2Cdev::readBytes(devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setZAccelOffset(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU6050_RA_ZA_OFFS_H, offset);
}
// XG_OFFS_USR*寄存器
int16_t MPU6050::getXGyroOffsetUser() {
I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setXGyroOffsetUser(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
}
//YG_OFFS_USR*寄存器
int16_t MPU6050::getYGyroOffsetUser() {
I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setYGyroOffsetUser(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
}
// ZG_OFFS_USR* 寄存器
int16_t MPU6050::getZGyroOffsetUser() {
I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setZGyroOffsetUser(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
}
// INT_ENABLE 寄存器（DMP函数）
bool MPU6050::getIntPLLReadyEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
return buffer[0];
}
void MPU6050::setIntPLLReadyEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
}
bool MPU6050::getIntDMPEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
return buffer[0];
}
void MPU6050::setIntDMPEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
}
// DMP_INT_STATUS
bool MPU6050::getDMPInt5Status() {
I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
return buffer[0];
}
bool MPU6050::getDMPInt4Status() {
I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer);
return buffer[0];
}
bool MPU6050::getDMPInt3Status() {
I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer);
return buffer[0];
}
bool MPU6050::getDMPInt2Status() {
I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer);
return buffer[0];
}
bool MPU6050::getDMPInt1Status() {
I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer);
return buffer[0];
}
bool MPU6050::getDMPInt0Status() {
I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer);
return buffer[0];
}
// INT_STATUS寄存器（DMP函数）
bool MPU6050::getIntPLLReadyStatus() {
I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
return buffer[0];
}
bool MPU6050::getIntDMPStatus() {
I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
return buffer[0];
}
// USER_CTRL寄存器（DMP函数）
bool MPU6050::getDMPEnabled() {
I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer);
return buffer[0];
}
void MPU6050::setDMPEnabled(bool enabled) {
I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
}
void MPU6050::resetDMP() {
I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
}
// BANK_SEL寄存器
void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
bank &= 0x1F;
if (userBank) bank |= 0x20;
if (prefetchEnabled) bank |= 0x40;
I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
}
// MEM_START_ADDR寄存器
void MPU6050::setMemoryStartAddress(uint8_t address) {
I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
}
// MEM_R_W 寄存器
uint8_t MPU6050::readMemoryByte() {
I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer);
return buffer[0];
}
void MPU6050::writeMemoryByte(uint8_t data) {
I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data);
}
void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
setMemoryBank(bank);
setMemoryStartAddress(address);
uint8_t chunkSize;
for (uint16_t i = 0; i < dataSize;) {
// 根据库的位置和数据大小确定正确的语块大小
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
// 确保没有超过数据的大小
if (i + chunkSize > dataSize) chunkSize = dataSize - i;
// 确保这一语块没有超过库的界限（256字节）
if (chunkSize > 256 - address) chunkSize = 256 - address;
// 读取指定数据块
I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
// 通过[语块大小]增加字节数
i += chunkSize;
// 在256中uint8_t自动变为0
address += chunkSize;
// 如果无法完成，更新库（如果有必要的话）和地址
if (i < dataSize) {
if (address == 0) bank++;
setMemoryBank(bank);
setMemoryStartAddress(address);
}
}
}
bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
setMemoryBank(bank);
setMemoryStartAddress(address);
uint8_t chunkSize;
uint8_t *verifyBuffer;
uint8_t *progBuffer;
uint16_t i;
uint8_t j;
if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
for (i = 0; i < dataSize;) {
// 根据库的位置和数据大小确定正确的语块大小
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
// 确保没有超过数据的大小
if (i + chunkSize > dataSize) chunkSize = dataSize - i;
// 确保这一语块没有超过库的界限（256字节）
if (chunkSize > 256 - address) chunkSize = 256 - address;
if (useProgMem) {
// 编写指定数据块
for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
} else {
// 编写指定数据块
progBuffer = (uint8_t *)data + i;
}
I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
// 如果有必要，请验证数据
if (verify && verifyBuffer) {
setMemoryBank(bank);
setMemoryStartAddress(address);
I2Cdev::readBytes(devAddr, MPU6050_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.
}
}
// 通过[语块大小]增加字节数
i += chunkSize;
// 在256中uint8_t自动变为0
address += chunkSize;
// 如果无法完成，更新库（如果有必要的话）和地址
if (i < dataSize) {
if (address == 0) bank++;
setMemoryBank(bank);
setMemoryStartAddress(address);
}
}
if (verify) free(verifyBuffer);
if (useProgMem) free(progBuffer);
return true;
}
bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
return writeMemoryBlock(data, dataSize, bank, address, verify, true);
}
bool MPU6050::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); // 假设一个8字节的字节块,如果有必要，进行重新分配。
}
// 配置的设置数据是一串以[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++];
}
// 编写数据或运行特殊操作
if (length > 0) {
// 编写常规数据块
/*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 = writeMemoryBlock(progBuffer, length, bank, offset, true);
i += length;
} else {
// 特殊指令
// 注意：这一操作（什么时候做什么事）是完全没有成文的，即不确定的。这段代码也只适于读取，至于为什么把这段代码放在这（甚至是否应该放在这），仍然是现在猜测和研究的对象。
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) {
// 启用DMP相关中断
//设置零运动中断启用（真）;
//设置FIFO缓冲区溢出启用（真）;
//设置DMP启用（真）;
I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32); // single operation
success = true;
} else {
// 未知的特殊指令
success = false;
}
}
if (!success) {
if (useProgMem) free(progBuffer);
return false; // uh oh
}
}
if (useProgMem) free(progBuffer);
return true;
}
bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
return writeDMPConfigurationSet(data, dataSize, true);
}
// DMP_CFG_1寄存器
uint8_t MPU6050::getDMPConfig1() {
I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer);
return buffer[0];
}
void MPU6050::setDMPConfig1(uint8_t config) {
I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
}
// DMP_CFG_2寄存器
uint8_t MPU6050::getDMPConfig2() {
I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer);
return buffer[0];
}
void MPU6050::setDMPConfig2(uint8_t config) {
I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
}[/code] |
|
发表于 2013-5-22 21:56:17
发表于 2013-5-22 23:07:47
楼主