/* @brief Get angle Z,X calculated data
*
* @param Angle - calculated angle
*
* @retval void
*/
inline MPU6050_errorstatus MPU6050_Get_AccAngleXZ_Data( float* Angle ){
MPU6050_errorstatus errorstatus = MPU6050_NO_ERROR;
uint8_t xlow,xhigh, zlow, zhigh;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_L, &xlow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_H, &xhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_L, &zlow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_H, &zhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
int16_t X = (xhigh << 8 | xlow);
int16_t Z = (zhigh << 8 | zlow);
float tempAngle = 0.0f;
tempAngle = radiansToDegrees( atanf( ( (float)Z )/ ( (float)X ) ) );
if( ( Z < 0 && tempAngle < 0 )
|| ( Z > 0 && tempAngle > 0 )
)
{
*Angle = tempAngle;
}
else if( Z > 0 && tempAngle < 0 )
{
*Angle = 180 + tempAngle;
}
else if( Z < 0 && tempAngle > 0 )
{
*Angle = -180 + tempAngle;
}
*Angle += ANGLE_OFFSET;
return errorstatus;
}
void Gyro_OFFSET(void)
{
uint16_t cnt_g=0;
int32_t tempgx=100,tempgy=100,tempgz=100;
int16_t gx_last=0,gy_last=0,gz_last=0;
sensor.gyro.quiet.x=0;
sensor.gyro.quiet.y=0;
sensor.gyro.quiet.z=0;
while(tempgx>=100 || tempgy>=100 || tempgz>=100) //此循环是确保四轴处于完全静止状态
{
tempgx=0;tempgy=0;tempgz=0;cnt_g=100;
while(cnt_g--)
{
MPU6050_Read();
sensor.gyro.origin.x = ((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]);
sensor.gyro.origin.y = ((((int16_t)mpu6050_buffer[10]) << 8)| mpu6050_buffer[11]);
sensor.gyro.origin.z = ((((int16_t)mpu6050_buffer[12]) << 8)| mpu6050_buffer[13]);
tempgx += FL_ABS(sensor.gyro.origin.x - gx_last);
tempgy += FL_ABS(sensor.gyro.origin.y - gy_last);
tempgz += FL_ABS(sensor.gyro.origin.z - gz_last);
gx_last = sensor.gyro.origin.x;
gy_last = sensor.gyro.origin.y;
gz_last = sensor.gyro.origin.z;
}
}
tempgx=0;tempgy=0;tempgz=0;cnt_g=2000;
while(cnt_g--) //此循环进行陀螺仪标定，前提是四轴已经处于完全静止状态
{
MPU6050_Read();
sensor.gyro.origin.x = ((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]);
sensor.gyro.origin.y = ((((int16_t)mpu6050_buffer[10]) << 8)| mpu6050_buffer[11]);
sensor.gyro.origin.z = ((((int16_t)mpu6050_buffer[12]) << 8)| mpu6050_buffer[13]);
tempgx += sensor.gyro.origin.x;
tempgy += sensor.gyro.origin.y;
tempgz += sensor.gyro.origin.z;
}
sensor.gyro.quiet.x = tempgx/2000;
sensor.gyro.quiet.y = tempgy/2000;
sensor.gyro.quiet.z = tempgz/2000;
}
void Prepare_Data(void)
{
static uint8_t filter_cnt=0;
int32_t temp1=0,temp2=0,temp3=0;
uint8_t i;
MPU6050_Read(); //将匿名的硬件I2C改为模拟I2C
MPU6050_Dataanl();//对6050数据处理，减去零偏。若未计算零偏，则计算
ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;//更新滑动窗口数组
ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;
ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;
for(i=0;i<FILTER_NUM;i++) //20个数据，求和
{
temp1 += ACC_X_BUF[i];
temp2 += ACC_Y_BUF[i];
temp3 += ACC_Z_BUF[i];
}
ACC_AVG.X = temp1 / FILTER_NUM; //求平均
ACC_AVG.Y = temp2 / FILTER_NUM;
ACC_AVG.Z = temp3 / FILTER_NUM;
filter_cnt++;
if(filter_cnt==FILTER_NUM) filter_cnt=0;
//已使用四元数，不对x，y轴积分。
//GYRO_I.X += MPU6050_GYRO_LAST.X*Gyro_G*0.001;//0.001是时间间隔,两次prepare的执行周期
//GYRO_I.Y += MPU6050_GYRO_LAST.Y*Gyro_G*0.001;
GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.001;
}
void State_Update(float dt)
{
MPU6050_Read();
MPU_GetAccValue();
MPU_GetGyroRate();
AHRSupdateIMU(gyro_x_rate*M_PI/180,gyro_y_rate*M_PI/180,gyro_z_rate*M_PI/180, acc_x_temp,acc_y_temp,acc_z_temp,dt);
AHRS_GetRPY();
/*ADNS3080_Read();*/
}
/* @brief Get angle/s around Y axis dgr/s
*
* @param Y - sensor Y gyroRaw(directly from sensor
* @param DegPerSecond - measurend data in degrees per second
*
* @retval void
*/
inline MPU6050_errorstatus MPU6050_Get_GyroY_Data( float *DegPerSecond )
{
MPU6050_errorstatus errorstatus = MPU6050_NO_ERROR;
uint8_t ylow, yhigh;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_YOUT_L, &ylow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_YOUT_H, &yhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
uint16_t Range = 1000.0f;
int16_t Y = (int16_t)( yhigh << 8 | ylow );
float Last = 0.0f;
*DegPerSecond = ( float )( ( ( float ) Y * Range ) / ( 1 << 15 ) );
if( ( *DegPerSecond > 7.6f && *DegPerSecond < 7.87f)
|| ( *DegPerSecond < -7.6f && *DegPerSecond > -7.87f) )
{
if( Last < 1 && Last > -1 )
{
*DegPerSecond = Last;
}
else
{
Last = *DegPerSecond;
}
}
*DegPerSecond -= GYRO_OFFSET;
return errorstatus;
}
/* @brief Get Accelerometer full scale range
* Reads the Accel_CONFIG register and returns the value of accelerometer's range
*
* @retval tmp - value of accelerometer's range
*/
uint8_t MPU6050_Accel_Get_Range(void){
MPU6050_errorstatus errorstatus;
uint8_t tmp;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_CONFIG, &tmp, 1);
if(errorstatus != 0){
return 1;
}
else return tmp;
}
/* @brief Test if chip is visible on I2C line
* Reads the WHO_AM_I register
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Test(void){
MPU6050_errorstatus errorstatus;
uint8_t tmp;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, WHO_AM_I, &tmp, 1);
if(tmp != (uint8_t)0x68){
return errorstatus;
}
return MPU6050_NO_ERROR;
}
/**************************实现函数********************************************
//将iic读取到得数据分拆,放入相应寄存器
*******************************************************************************/
void MPU6050_Dataanl(void)
{
MPU6050_Read();
sensor.acc.origin.x = ((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1]) - sensor.acc.quiet.x;
sensor.acc.origin.y = ((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3]) - sensor.acc.quiet.y;
sensor.acc.origin.z = ((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]);
sensor.gyro.origin.x = ((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]);
sensor.gyro.origin.y = ((((int16_t)mpu6050_buffer[10]) << 8)| mpu6050_buffer[11]);
sensor.gyro.origin.z = ((((int16_t)mpu6050_buffer[12]) << 8)| mpu6050_buffer[13]);
sensor.gyro.radian.x = sensor.gyro.origin.x * Gyro_Gr - sensor.gyro.quiet.x * Gyro_Gr;
sensor.gyro.radian.y = sensor.gyro.origin.y * Gyro_Gr - sensor.gyro.quiet.y * Gyro_Gr;
sensor.gyro.radian.z = sensor.gyro.origin.z * Gyro_Gr - sensor.gyro.quiet.z * Gyro_Gr;
////////////////////////////////////////////////////
// The calibration of acc //
////////////////////////////////////////////////////
if(accCorrect_flag)
{
static int32_t tempax=0,tempay=0,tempaz=0;
static uint8_t cnt_a=0;
LED_ALLON();
if(cnt_a==0)
{
sensor.acc.quiet.x = 0;
sensor.acc.quiet.y = 0;
sensor.acc.quiet.z = 0;
tempax = 0;
tempay = 0;
tempaz = 0;
cnt_a = 1;
}
tempax+= sensor.acc.origin.x;
tempay+= sensor.acc.origin.y;
tempaz+= sensor.acc.origin.z;
if(cnt_a==200)
{
sensor.acc.quiet.x = tempax/cnt_a;
sensor.acc.quiet.y = tempay/cnt_a;
sensor.acc.quiet.z = tempaz/cnt_a;
cnt_a = 0;
accCorrect_flag = 0;
EE_SAVE_ACC_OFFSET();//保存数据
return;
}
cnt_a++;
}
}
/* @brief Read MPU6050 temperature
* @retval temp_celsius - temperature in degrees celsius
*/
int16_t MPU6050_Get_Temperature(void){
MPU6050_errorstatus errorstatus;
uint8_t temp_low;
uint8_t temp_high;
int16_t temp;
int16_t temp_celsius;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, TEMP_OUT_L, &temp_low, 1);
if(errorstatus != 0){
return 1;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, TEMP_OUT_H, &temp_high, 1);
if(errorstatus != 0){
return 1;
}
temp = (uint16_t)(temp_high << 8 | temp_low);
temp_celsius = temp/340 + 36;
return temp_celsius;
}
/* @brief Get Accelerometer X,Y,Z raw data
*
* @param X - sensor accel on X axis
* @param Y - sensor accel on Y axis
* @param Z - sensor accel on Z axis
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Get_Accel_Data_Raw(int16_t* X, int16_t* Y, int16_t* Z){
MPU6050_errorstatus errorstatus;
uint8_t xlow, xhigh, ylow, yhigh, zlow, zhigh;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_L, &xlow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_H, &xhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_YOUT_L, &ylow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_YOUT_H, &yhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_L, &zlow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_H, &zhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
*X = (int16_t)(xhigh << 8 | xlow);
*Y = (int16_t)(yhigh << 8 | ylow);
*Z = (int16_t)(zhigh << 8 | zlow);
return errorstatus;
}
void Duty_2ms()
{
float inner_loop_time;
inner_loop_time = Get_Cycle_T(0); //获取内环准确的执行周期
test[0] = GetSysTime_us()/1000000.0f;
MPU6050_Read(); //读取mpu6轴传感器
MPU6050_Data_Prepare( inner_loop_time ); //mpu6轴传感器数据处理
CTRL_1( inner_loop_time ); //内环角速度控制。输入：执行周期，期望角速度，测量角速度，角度前馈；输出：电机PWM占空比。<函数未封装>
RC_Duty( inner_loop_time , Rc_Pwm_In ); // 遥控器通道数据处理 ，输入：执行周期，接收机pwm捕获的数据。
test[1] = GetSysTime_us()/1000000.0f;
}
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;
}
