单片机源程序如下:
- #include "public.h"
- #include "USART.h"
- #include "MPU6050.h"
- #include "TIM.h"
- #include "Filter.h"
- MPU_TypeDef MPU_Offset,MPU0;
- MPU_Angle_TypeDef MPU_Angle0;
- Angle_TypeDef Angle_Final;
- Angle_TypeDef Angle_CMP1,Angle_CMP2,Angle_CMP3,Angle_CMP4,Angle_CMP5;
- extern Filter_Kalman_TypeDef K1,K2;
- int main()
- {
- SystemInit();
-
- Printf_Init(57600); //串口打印
-
- MPU6050_Init(); //陀螺仪初始化
- printf("Waitting....");
- delay_ms(1000); //延时1000ms
- printf("OK..........");
- GetData_MPU(&MPU_Offset); //消除陀螺仪偏差
-
- Time_Init(TIM_3,12); //开启10ms定时器
-
- Kalman_Init(&K1);
- Kalman_Init(&K2);
-
- while(1)
- {
-
- // printf("%2.4lf,%2.4lf\r\n",Angle_Final.Pitch,Angle_Final.Roll);
- printf("%2.4lf,%2.4lf,%2.4lf,%2.4lf,%2.4lf,%2.4lf,%2.4lf,%2.4lf,%2.4lf\r\n",Angle_CMP1.Pitch,Angle_CMP2.Pitch,Angle_CMP3.Pitch,Angle_CMP4.Pitch,Angle_CMP5.Pitch,Angle_CMP1.Roll,Angle_CMP2.Roll,Angle_CMP3.Roll,Angle_CMP4.Roll);
-
-
- // GetData_MPU(&MPU0);
- // printf("Pitch:%d\t",MPU0.GYRx);
- // printf("Roll: %d\t",MPU0.GYRy);
- // printf("Yaw: %d\t",MPU0.GYRz);
- // GetData_MPU_Angle(&MPU_Angle0);
- // printf("%2.4lf\t",MPU_Angle0.ACC_Pitch);
- // printf("%2.4lf\t",MPU_Angle0.ACC_Roll);
- // printf("Pitch:%2.4lf\t",MPU_Angle0.GYR_Pitch);
- // printf("Roll: %2.4lf\t",MPU_Angle0.GYR_Roll);
- // printf("Yaw: %2.4lf\t",MPU_Angle0.GYR_Yaw);
- // printf("\r\n");
- // printf("%2.4lf %2.4lf %2.4lf %2.4lf %2.4lf \n",MPU_Angle0.ACC_Pitch,MPU_Angle0.ACC_Roll,MPU_Angle0.GYR_Pitch,MPU_Angle0.GYR_Roll,MPU_Angle0.GYR_Yaw);
- // delay_ms(1000);
- }
- }
所有资料51hei提供下载:
A3.MPU605陀螺仪(硬件IIC).7z
(207.4 KB, 下载次数: 111)
|
