//读gyro
void MPU6050GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
IICreadBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buf);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) ;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) ;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) ;
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_getRaw(int16_t *x,int16_t *y,int16_t *z)
*功 能: 写HMC5883L的寄存器
输入参数： reg 寄存器地址
val 要写入的值
输出参数： 无
*******************************************************************************/
void HMC58X3_getRaw(int16_t *x,int16_t *y,int16_t *z) {
unsigned char vbuff[6];
vbuff[0]=vbuff[1]=vbuff[2]=vbuff[3]=vbuff[4]=vbuff[5]=0;
IICreadBytes(HMC58X3_ADDR,HMC58X3_R_XM,6,vbuff);
HMC58X3_newValues(((int16_t)vbuff[0] << 8) | vbuff[1],((int16_t)vbuff[4] << 8) | vbuff[5],((int16_t)vbuff[2] << 8) | vbuff[3]);
*x = HMC5883_FIFO[0][10];
*y = HMC5883_FIFO[1][10];
*z = HMC5883_FIFO[2][10];
}
//读acc
void MPU6050AccRead(int16_t *accData)
{
uint8_t buf[6];
IICreadBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buf);
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
}
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;
}
/**************************实现函数********************************************
*函数原型: uint8_t MPU6050_getDeviceID(void)
*功 能: 读取 MPU6050 WHO_AM_I 标识 将返回 0x68
*******************************************************************************/
uint8_t MPU6050_getDeviceID(void) {
IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
return buffer[0];
}
int16_t MPU6050_getZGyroOffset(void) {
IICreadBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
// MEM_R_W register
uint8_t MPU6050_readMemoryByte(void) {
IICreadBytes(devAddr, MPU6050_RA_MEM_R_W, 1 , buffer);
return buffer[0];
}
int16_t MPU6050_getYGyroOffset(void) {
IICreadBytes(MPU6050_ADDR, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}