void MPU6050_setGyroOffset(int16_t offset[3])
{
uint8_t buf[2],i=0;
for(i=0; i<3; i++) {
buf[0]=offset[i]>>8;
buf[1]=offset[i];
IICwriteBytes(devAddr, MPU6050_RA_XG_OFFS_USRH + i*2, 2,buf);
}
}
//用于校准 DMP的偏置值
void MPU6050_setAccOffset(int16_t offset[3])
{
uint8_t buf[2],i=0;
for(i=0;i<3;i++)
{
buf[0]=offset[i]>>8;
buf[1]=offset[i];
IICwriteBytes(MPU6050_ADDR, MPU6050_RA_XA_OFFS_H + i*2, 2,buf);
}
}
uint8_t MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem)
{
uint8_t chunkSize;
//uint8_t *verifyBuffer;
uint8_t *tprogBuffer;
uint16_t i;
uint8_t j;
MPU6050_setMemoryBank(bank,0,0);
MPU6050_setMemoryStartAddress(address);
for (i = 0; i < dataSize;) {
// determine correct chunk size according to bank position and data size
chunkSize = MPU6050_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;
// write the chunk of data as specified
tprogBuffer = (uint8_t*)data + i;
IICwriteBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, tprogBuffer);
// verify data if needed
if (verify) {
MPU6050_setMemoryBank(bank,0,0);
MPU6050_setMemoryStartAddress(address);
IICreadBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
for(j=0; j<chunkSize; j++) {
if(tprogBuffer[j] != verifyBuffer[j])
return 0; // uh oh.
}
}
// increase byte index by [chunkSize]
i += chunkSize;
// uint8_tautomatically 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++;
MPU6050_setMemoryBank(bank,0,0);
MPU6050_setMemoryStartAddress(address);
}
}
return 1;
}
/**************************实现函数********************************************
*函数原型: unsigned char IICwriteByte(unsigned char dev, unsigned char reg, unsigned char data)
*功 能: 写入指定设备 指定寄存器一个字节
输入 dev 目标设备地址
reg 寄存器地址
data 将要写入的字节
返回 1
*******************************************************************************/
unsigned char IICwriteByte(unsigned char dev, unsigned char reg, unsigned char data){
return IICwriteBytes(dev, reg, 1, &data);
}
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
return IICwriteBytes(addr_, reg_, 1, &data);
}