/* @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; }