int I2C_Read(uint8_t addr,void *pbuffer,uint16_t len)
{
struct i2c_job_st i2c_job;
xSemaphoreTake(xSemaphoreI2C_Mutex,portMAX_DELAY);
if (I2C_isBusy())
{
i2c_error_flags = I2C_SR1_SB;
xSemaphoreGive(xSemaphoreI2C_Mutex);
return FALSE;
}
i2c_cntr = 0;
i2c_oper = I2C_Direction_Receiver;
job = &i2c_job;
i2c_addr = addr << 1;
i2c_job.buf = (uint8_t *)pbuffer;
i2c_job.len = len;
i2c_job.dir = I2C_Direction_Receiver;
i2c_job.last = TRUE;
i2c_done = i2c_err = FALSE;
// clear the semaphore
while (xSemaphoreTake(xSemaphoreI2C_Work,0));
if ((i2c_oper == I2C_Direction_Receiver) && (len == 2))
I2C_NACKPositionConfig(I2Cx,I2C_NACKPosition_Next);
else
I2C_NACKPositionConfig(I2Cx,I2C_NACKPosition_Current);
I2C_AcknowledgeConfig(I2Cx,ENABLE);
I2C_ITConfig(I2Cx,I2C_IT_BUF | I2C_IT_ERR | I2C_IT_EVT,ENABLE);
/* Send START condition */
I2C_GenerateSTART(I2Cx, ENABLE);
while (!i2c_done && !i2c_err)
{
if (!xSemaphoreTake(xSemaphoreI2C_Work,SEMA_DELAY))
{
I2C_Open(0);
i2c_err = TRUE;
break;
}
}
I2C_ITConfig(I2Cx,I2C_IT_BUF | I2C_IT_ERR | I2C_IT_EVT,DISABLE);
xSemaphoreGive(xSemaphoreI2C_Mutex);
return !i2c_err;
}
void MPU6050_Read(void)
{
__loop_cnt_gyro__ = (__loop_cnt_gyro__ < MPU6050_GYRO_BUFFER_LEN - 1 ? (__loop_cnt_gyro__ + 1) : 0);
__loop_cnt_acc__ = (__loop_cnt_acc__ < MPU6050_ACC_BUFFER_LEN - 1 ? (__loop_cnt_acc__ + 1) : 0);
uint8_t buffer[14];
I2C_ReadData(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, buffer, 14);
while(I2C_isBusy());
__average_buf_acc__[__loop_cnt_acc__][0] = (int16_t)((((uint16_t)buffer[0])<<8) | ((uint16_t)buffer[1]));// - __MPU6050_ACC_OFFSET.x;
__average_buf_acc__[__loop_cnt_acc__][1] = (int16_t)((((uint16_t)buffer[2])<<8) | ((uint16_t)buffer[3]));// - __MPU6050_ACC_OFFSET.y;
__average_buf_acc__[__loop_cnt_acc__][2] = (int16_t)((((uint16_t)buffer[4])<<8) | ((uint16_t)buffer[5]));// - __MPU6050_ACC_OFFSET.z;
__average_buf_gyro__[__loop_cnt_gyro__][0] = (int16_t)((((uint16_t)buffer[8])<<8) | ((uint16_t)buffer[9])) - __MPU6050_GYRO_OFFSET.x;
__average_buf_gyro__[__loop_cnt_gyro__][1] = (int16_t)((((uint16_t)buffer[10])<<8) | ((uint16_t)buffer[11])) - __MPU6050_GYRO_OFFSET.y;
__average_buf_gyro__[__loop_cnt_gyro__][2] = (int16_t)((((uint16_t)buffer[12])<<8) | ((uint16_t)buffer[13])) - __MPU6050_GYRO_OFFSET.z;
uint16tofloat();
}
uint8_t MPU6050_SelfAdjust(void)
{
uint8_t buffer[14]; //iic读取后存放数据
int32_t avg_value[7];
//////////////////////////////////////////////////////////////////
/////////////////////// Normal output //////////////////////////
//////////////////////////////////////////////////////////////////
avg_value[0] = avg_value[1] = avg_value[2] = avg_value[3]
= avg_value[4] = avg_value[5] = avg_value[6] = 0;
uint32_t t = 0;
__fixLoopPeriod(0,&t);
for(int i = 0 ; i < 1024 ; i ++){
__fixLoopPeriod(2000,&t);
I2C_ReadData(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, buffer, 14);
while(I2C_isBusy());
avg_value[0] += (int16_t)((((uint16_t)buffer[0])<<8) | ((uint16_t)buffer[1]));
avg_value[1] += (int16_t)((((uint16_t)buffer[2])<<8) | ((uint16_t)buffer[3]));
avg_value[2] += (int16_t)((((uint16_t)buffer[4])<<8) | ((uint16_t)buffer[5]));
avg_value[4] += (int16_t)((((uint16_t)buffer[8])<<8) | ((uint16_t)buffer[9]));
avg_value[5] += (int16_t)((((uint16_t)buffer[10])<<8) | ((uint16_t)buffer[11]));
avg_value[6] += (int16_t)((((uint16_t)buffer[12])<<8) | ((uint16_t)buffer[13]));
}
float tmpx,tmpy,tmpz;
tmpx = ((float)avg_value[0])*(ACC_MAX_SCALE/32768.0f/1024.0f*9.8f);
tmpy = ((float)avg_value[1])*(ACC_MAX_SCALE/32768.0f/1024.0f*9.8f);
tmpz = ((float)avg_value[2])*(ACC_MAX_SCALE/32768.0f/1024.0f*9.8f);
attitude_gravity.x = (-tmpx + tmpy)*0.7071067812;
attitude_gravity.y = (tmpx + tmpy)*0.7071067812;
attitude_gravity.z = -tmpz;
__MPU6050_GYRO_OFFSET.x = avg_value[4] >> 10;
__MPU6050_GYRO_OFFSET.y = avg_value[5] >> 10;
__MPU6050_GYRO_OFFSET.z = avg_value[6] >> 10;
///////////////////////////////////////////////////////////////////////////////////////
for(int i = 0 ; i < MPU6050_ACC_BUFFER_LEN || i < MPU6050_GYRO_BUFFER_LEN ; i++){
__fixLoopPeriod(2000,&t);
MPU6050_Read();
}
return 0;
}
int I2C_RandRead(uint8_t slave,uint16_t addr,uint8_t addrsize,void *pbuffer,uint16_t len)
{
struct i2c_job_st i2c_jobs[2];
uint8_t buf_offset[2];
xSemaphoreTake(xSemaphoreI2C_Mutex,portMAX_DELAY);
if (I2C_isBusy())
{
i2c_error_flags = I2C_SR1_SB;
xSemaphoreGive(xSemaphoreI2C_Mutex);
return FALSE;
}
i2c_cntr = 0;
i2c_oper = I2C_Direction_Transmitter;
job = i2c_jobs;
i2c_addr = slave << 1;
memset(i2c_jobs,0,sizeof(i2c_jobs));
if (addrsize == 1)
buf_offset[0] = (uint8_t)addr;
else
{
buf_offset[0] = (uint8_t)(addr >> 8);
buf_offset[1] = (uint8_t)(addr & 0xff);
}
i2c_jobs[0].buf = buf_offset;
i2c_jobs[0].len = addrsize;
i2c_jobs[0].dir = I2C_Direction_Transmitter;
i2c_jobs[1].buf = (uint8_t *)pbuffer;
i2c_jobs[1].len = len;
i2c_jobs[1].dir = I2C_Direction_Receiver;
i2c_jobs[1].last = TRUE;
i2c_error_flags = 0;
i2c_done = i2c_err = FALSE;
// clear the semaphore
while (xSemaphoreTake(xSemaphoreI2C_Work,0));
I2C_AcknowledgeConfig(I2Cx,ENABLE);
I2C_NACKPositionConfig(I2Cx,I2C_NACKPosition_Current);
I2C_ITConfig(I2Cx,I2C_IT_BUF | I2C_IT_ERR | I2C_IT_EVT,ENABLE);
/* Send START condition */
I2C_GenerateSTART(I2Cx, ENABLE);
while (!i2c_done && !i2c_err)
{
if (!xSemaphoreTake(xSemaphoreI2C_Work,SEMA_DELAY))
{
I2C_Open(0);
i2c_err = TRUE;
break;
}
}
I2C_ITConfig(I2Cx,I2C_IT_BUF | I2C_IT_ERR | I2C_IT_EVT,DISABLE);
xSemaphoreGive(xSemaphoreI2C_Mutex);
return !i2c_err;
}
