#include "sys.h"
#include "usart.h"
#include "delay.h"
#include "led.h"   
#include "lcd.h"  
#include "key.h"   
#include "mpu6050.h" 
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "math.h"


#define pi 3.141592653

//函数声明

void  mpu6050_dmp(void);//MPU6050状态量输出
void Kalman_Filter(float Gyro,float Accel);
//函数变量

float Angle=0,Gyro_x=0;         //小车滤波后倾斜角度/角速度
//******卡尔曼参数************

float  Q_angle=0.001;  
float  Q_gyro=0.003;
float  R_angle=0.5;
float  dt=0.005;                  //dt为kalman滤波器采样时间;
char   C_0 = 1;
float  Q_bias=0, Angle_err=0;
float  Ct_0=0, PCt_1=0, E=0;
float  K_0=0, K_1=0, t_0=0, t_1=0;
float  dot[4] ={0,0,0,0};
float  P[2][2] = { { 1, 0 },{ 0, 1 } };
//全局变量
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
short temp;     //温度  
float Ax,Ay,Az;//单位 g(9.8m/s^2)
float Gx,Gy,Gz;//单位 °/s
  float Angle_accX,Angle_accY,Angle_accZ;//存储加速度计算出的角度
  float Angle_gyroX,Angle_gyroY,Angle_gyroZ;//存储角速度计算出的角度

 int main(void)
 { 

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
delay_init();     //延时函数初始化  
uart_init(9600); //串口初始化为9600
LED_Init();   //初始化与LED连接的硬件接口
KEY_Init(); //初始化按键
LCD_Init();   //初始化LCD  
MPU_Init(); //初始化MPU6050


while(mpu_dmp_init())//监测模块是否正常；
  {
LCD_ShowString(30,130,200,16,16,"MPU6050 Error");
delay_ms(500);
}  

POINT_COLOR=BLUE;//设置字体为蓝 {MOD} 
  LCD_ShowString(30,80,200,16,16," aacx:    . C");
  LCD_ShowString(30,100,200,16,16,"gyrox:    . C");
  LCD_ShowString(30,120,200,16,16," aacy:    . C");  
  LCD_ShowString(30,140,200,16,16," gyroy :  . C");
LCD_ShowString(30,160,200,16,16," aacz:    . C");
  LCD_ShowString(30,180,200,16,16,"gyroz:    . C");

 
  LCD_ShowString(30,200,200,16,16," Temp:    . C");
  LCD_ShowString(30,220,200,16,16,"Pitch:    . C");
  LCD_ShowString(30,240,200,16,16," Roll:    . C");  
  LCD_ShowString(30,260,200,16,16," Yaw :    . C");  
  while(1)
{
//======一下三行是对加速度进行量化，得出单位为g的加速度值-2g量程
Ax=aacx/16384.00;
   Ay=aacy/16384.00;
   Az=aacz/16384.00;
   //==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
   Angle_accY= atan(Ax / Az)*180/pi;     //加速度仪，反正切获得弧度值，乘以180度/pi 
   Angle_accX= atan(Ay / Az)*180/ pi;   //获得角度值，乘以-1得正
   //==========以下三行是对角速度做量化-2000°量程==========
   Gx=gyrox/16.38;
   Gy=gyroy/16.38;
   Gz=gyroz/16.38;

 Kalman_Filter(Gy,Angle_accX);




mpu6050_dmp();

  if(Angle_accX<0){
LCD_ShowChar(30+48,20,'-',16,0);     //显示负号
Angle_accX=-Angle_accX; }               //转为正数
else LCD_ShowChar(30+48,20,' ',16,0); //去掉负号 
 
LCD_ShowNum(30+48+8,20,Angle_accX,5,16); //显示整数部分

if(Gx<0){
 LCD_ShowChar(30+48,40,'-',16,0);     //显示负号
Gx=-Gx;}             //转为正数
else LCD_ShowChar(30+48,40,' ',16,0); //去掉负号

LCD_ShowNum(30+48+8,40,Gx,5,16); //显示整数部分

if(Angle<0){
 LCD_ShowChar(30+48,60,'-',16,0);     //显示负号
Angle=-Angle;}             //转为正数
else LCD_ShowChar(30+48,60,' ',16,0); //去掉负号

LCD_ShowNum(30+48+8,60,Angle,5,16); //显示整数部分


}
}
 



void  mpu6050_dmp(void)
{
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{ 
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据


// if(temp<0)
// {
// LCD_ShowChar(30+48,200,'-',16,0); //显示负号
// temp=-temp; //转为正数
// }
// else 
// LCD_ShowChar(30+48,200,' ',16,0); //去掉负号 
// LCD_ShowNum(30+48+8,200,temp/100,3,16); //显示整数部分    
// LCD_ShowNum(30+48+40,200,temp%10,1,16); //显示小数部分

temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0); //显示负号
temp=-temp; //转为正数
}else
LCD_ShowChar(30+48,220,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,220,temp/10,3,16); //显示整数部分    
LCD_ShowNum(30+48+40,220,temp%10,1,16); //显示小数部分 

temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else
LCD_ShowChar(30+48,240,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,240,temp/10,3,16); //显示整数部分    
LCD_ShowNum(30+48+40,240,temp%10,1,16); //显示小数部分 

temp=yaw*10;
if(temp<0)
{
LCD_ShowChar(30+48,260,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else 
LCD_ShowChar(30+48,260,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,260,temp/10,3,16); //显示整数部分    
LCD_ShowNum(30+48+40,260,temp%10,1,16); //显示小数部分 

        temp=aacx*10;
if(temp<0)
{
LCD_ShowChar(30+48,80,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else 
LCD_ShowChar(30+48,80,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,80,temp/10,5,16); //显示整数部分    


        temp=gyrox*10;
if(temp<0)
{
LCD_ShowChar(30+48,100,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else 
LCD_ShowChar(30+48,100,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,100,temp/10,5,16); //显示整数部分    

 temp=aacy*10;
if(temp<0)
{
LCD_ShowChar(30+48,120,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else 
LCD_ShowChar(30+48,120,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,120,temp/10,5,16); //显示整数部分     

        temp=gyroy*10;
if(temp<0)
{
LCD_ShowChar(30+48,140,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else 
LCD_ShowChar(30+48,140,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,140,temp/10,5,16); //显示整数部分    


temp=aacz*10;
if(temp<0)
{
LCD_ShowChar(30+48,160,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else 
LCD_ShowChar(30+48,160,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,160,temp/10,5,16); //显示整数部分    

        temp=gyroz*10;
if(temp<0)
{
LCD_ShowChar(30+48,180,'-',16,0); //显示负号
temp=-temp; //转为正数
}
else 
LCD_ShowChar(30+48,180,' ',16,0); //去掉负号 
LCD_ShowNum(30+48+8,180,temp/10,5,16); //显示整数部分    


}
}

void Kalman_Filter(float Gyro,float Accel)
{
Angle+=(Gyro - Q_bias) * dt;           //先验估计


Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;

PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;

Angle_err = Accel - Angle;

PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * PP[0][1];

PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;

Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_x   = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}