File content as of revision 0:9b8df4f9b792:

/***************************************************************
功能 : 基于mbed的动感平台SDK 包含摇杆 位置传感器 舵机 以及液晶屏等外设
作者 : 陈欢 清华大学电机工程与应用电子技术系
邮箱 : h-che14@mails.tsinghua.edu.cn OR heroistired@gmail.com
声明 : 
本程序仅供学习与交流使用,如需他用,须联系作者
All rights reserved
2017.2.15
***************************************************************/

#include "mbed.h"
#include "mpu6050_config.h"
#include "Servo.h"
#include "TFT_ILI9340.h"
#include "PinMap.h"
Serial SystemUart(SERIAL_TX, SERIAL_RX);  //系统串口初始化

//////////////////////////////////////////////////////////////////////////////////   
//头文件部分
//////////////////////////////////////////////////////////////////////////////////   

///////////////////////////////////////
//系统相关
///////////////////////////////////////

//系统监控 
//接受外部调用的变量
int TimeNow = 0;  //当前的系统时间
float CpuUsage = 0.0; //当前的CPU使用率 以主中断为基准
unsigned int MainInterruptCounter = 0; //存储系统主中断的总执行次数
float MainInterruptFreq = 0.0; //存储系统主中断的实际频率 Hz
//中间变量 不接受外部调用
int FreqTime = 0; //计算系统中断的实际频率 存储的是测量开始时的系统时间
int FreqCounter = 0; //计算系统中断的实际频率 存储的是测量开始时的中断执行次数
bool Flag_MeasureFreq = true;  //标志 是否要进行一次频率测量


///////////////////////////////////////
//mpuiic.h
///////////////////////////////////////

//声明IIC需要用到的IO口
DigitalOut MPU_IIC_SCL(Pin_MPU6050SCL); //SCL
DigitalInOut MPU_IIC_SDA(Pin_MPU6050SDA); //SDA 

//弥补屏幕LED画错的bug
DigitalOut TFT_LED(Pin_TFT_LED);

               
//定义IO操作的宏
#define MPU_SDA_IN()  MPU_IIC_SDA.input()
#define MPU_SDA_OUT() MPU_IIC_SDA.output() 
#define MPU_READ_SDA   MPU_IIC_SDA.read() //读取SDA

//兼容性宏定义
#define delay_us wait_us
#define delay_ms wait_ms
#define u8 unsigned char
#define u16 unsigned short 

//IIC所有操作函数
void MPU_IIC_Delay(void);               //MPU IIC延时函数
void MPU_IIC_Init(void);                //初始化IIC的IO口                 
void MPU_IIC_Start(void);               //发送IIC开始信号
void MPU_IIC_Stop(void);                //发送IIC停止信号
void MPU_IIC_Send_Byte(u8 txd);         //IIC发送一个字节
u8 MPU_IIC_Read_Byte(unsigned char ack);//IIC读取一个字节
u8 MPU_IIC_Wait_Ack(void);              //IIC等待ACK信号
void MPU_IIC_Ack(void);                 //IIC发送ACK信号
void MPU_IIC_NAck(void);                //IIC不发送ACK信号

void IMPU_IC_Write_One_Byte(u8 daddr,u8 addr,u8 data);
u8 MPU_IIC_Read_One_Byte(u8 daddr,u8 addr);  

///////////////////////////////////////
//mp6050.h
///////////////////////////////////////
u8 MPU_Init(void);                              //初始化MPU6050
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读 
u8 MPU_Write_Byte(u8 reg,u8 data);              //IIC写一个字节
u8 MPU_Read_Byte(u8 reg);                       //IIC读一个字节

u8 MPU_Set_Gyro_Fsr(u8 fsr);
u8 MPU_Set_Accel_Fsr(u8 fsr);
u8 MPU_Set_LPF(u16 lpf);
u8 MPU_Set_Rate(u16 rate);
u8 MPU_Set_Fifo(u8 sens);


short MPU_Get_Temperature(void);
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);

///////////////////////////////////////
//软件方法解算姿态
///////////////////////////////////////
Timer SystemTimer; //系统计时器
Ticker MainInterrupt;  //主中断 100Hz
//加速度 角速度 温度(原始值)
short aacx = 0, aacy = 0, aacz = 0, gyrox = 0, gyroy = 0, gyroz = 0, temp = 0;
float Pitch = 0, Yaw = 0, Roll = 0; //绕x,y,z轴的转角
float ThetaXz = 0, ThetaYz = 0; //重力加速度在XOZ、YOZ面上与z轴夹角

//卡尔曼滤波器相关参量
#define PI 3.1415926
struct KalmanStruct
{
    float K1; 
    float angle, angle_dot;     
    float Q_angle;// 过程噪声的协方差
    float Q_gyro;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
    float R_angle;// 测量噪声的协方差 既测量偏差
    float dt;//                 
    char  C_0;
    float Q_bias, Angle_err;
    float PCt_0, PCt_1, E;
    float K_0, K_1, t_0, t_1;
    float Pdot[4];
    float PP[2][2];
};

struct KalmanStruct X_Axis = {0}, Y_Axis = {0};

void MainInterruptIRQ(void);  //主中断服务函数
void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY); //卡尔曼滤波器
void Processing(void);  //用户进程函数

//系统初始化函数
void System_Init(void);

//////////////////////////////////////////////////////////////////////////////////   
//函数实现部分
//////////////////////////////////////////////////////////////////////////////////   

///////////////////////////////////////
//mpuiic.c
///////////////////////////////////////

//MPU IIC 延时函数
void MPU_IIC_Delay(void)
{
    delay_us(2);
}
//初始化IIC
void MPU_IIC_Init(void)
{                        
    RCC->APB2ENR|=1<<4;     //先使能外设IO PORTC时钟                            
    GPIOC->CRH&=0XFFF00FFF; //PC11/12 推挽输出
    GPIOC->CRH|=0X00033000;    
    GPIOC->ODR|=3<<11;      //PC11,12 输出高
}
//产生IIC起始信号
void MPU_IIC_Start(void)
{
    MPU_SDA_OUT();     //sda线输出
    MPU_IIC_SDA=1;        
    MPU_IIC_SCL=1;
    MPU_IIC_Delay();
    MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low 
    MPU_IIC_Delay();
    MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据 
}     
//产生IIC停止信号
void MPU_IIC_Stop(void)
{
    MPU_SDA_OUT();//sda线输出
    MPU_IIC_SCL=0;
    MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
    MPU_IIC_Delay();
    MPU_IIC_SCL=1;  
    MPU_IIC_SDA=1;//发送I2C总线结束信号
    MPU_IIC_Delay();                                
}
//等待应答信号到来
//返回值：1，接收应答失败
//        0，接收应答成功
u8 MPU_IIC_Wait_Ack(void)
{
    u8 ucErrTime=0;
    MPU_SDA_IN();      //SDA设置为输入  
    MPU_IIC_SDA=1;MPU_IIC_Delay();     
    MPU_IIC_SCL=1;MPU_IIC_Delay();   
    while(MPU_READ_SDA)
    {
        ucErrTime++;
        if(ucErrTime>250)
        {
            MPU_IIC_Stop();
            return 1;
        }
    }
    MPU_IIC_SCL=0;//时钟输出0      
    return 0;  
} 
//产生ACK应答
void MPU_IIC_Ack(void)
{
    MPU_IIC_SCL=0;
    MPU_SDA_OUT();
    MPU_IIC_SDA=0;
    MPU_IIC_Delay();
    MPU_IIC_SCL=1;
    MPU_IIC_Delay();
    MPU_IIC_SCL=0;
}
//不产生ACK应答          
void MPU_IIC_NAck(void)
{
    MPU_IIC_SCL=0;
    MPU_SDA_OUT();
    MPU_IIC_SDA=1;
    MPU_IIC_Delay();
    MPU_IIC_SCL=1;
    MPU_IIC_Delay();
    MPU_IIC_SCL=0;
}                                        
//IIC发送一个字节
//返回从机有无应答
//1，有应答
//0，无应答           
void MPU_IIC_Send_Byte(u8 txd)
{                        
    u8 t;   
    MPU_SDA_OUT();      
    MPU_IIC_SCL=0;//拉低时钟开始数据传输
    for(t=0;t<8;t++)
    {              
        MPU_IIC_SDA=(txd&0x80)>>7;
        txd<<=1;      
        MPU_IIC_SCL=1;
        MPU_IIC_Delay(); 
        MPU_IIC_SCL=0;  
        MPU_IIC_Delay();
    }    
}       
//读1个字节，ack=1时，发送ACK，ack=0，发送nACK   
u8 MPU_IIC_Read_Byte(unsigned char ack)
{
    unsigned char i,receive=0;
    MPU_SDA_IN();//SDA设置为输入
    for(i=0;i<8;i++ )
    {
        MPU_IIC_SCL=0; 
        MPU_IIC_Delay();
        MPU_IIC_SCL=1;
        receive<<=1;
        if(MPU_READ_SDA)receive++;   
        MPU_IIC_Delay(); 
    }                    
    if (!ack)
        MPU_IIC_NAck();//发送nACK
    else
        MPU_IIC_Ack(); //发送ACK   
    return receive;
}

///////////////////////////////////////
//mp6050.c
///////////////////////////////////////
//初始化MPU6050
//返回值:0,成功
//    其他,错误代码
u8 MPU_Init(void)
{ 
    u8 res; 
    MPU_IIC_Init();//初始化IIC总线
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
    delay_ms(100);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050 
    MPU_Set_Gyro_Fsr(3);                    //陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);                   //加速度传感器,±2g
    MPU_Set_Rate(200);                      //设置采样率200Hz
    MPU_Write_Byte(MPU_INT_EN_REG,0X00);    //关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);   //关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
    res=MPU_Read_Byte(MPU_DEVICE_ID_REG); 
    if(res==MPU_ADDR)//器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
        MPU_Set_Rate(200);                      //设置采样率为200Hz
    }else return 1;
    return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
    return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围  
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
    return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围  
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_LPF(u16 lpf)
{
    u8 data=0;
    if(lpf>=188)data=1;
    else if(lpf>=98)data=2;
    else if(lpf>=42)data=3;
    else if(lpf>=20)data=4;
    else if(lpf>=10)data=5;
    else data=6; 
    return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器  
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Rate(u16 rate)
{
    u8 data;
    if(rate>1000)rate=1000;
    if(rate<4)rate=4;
    data=1000/rate-1;
    data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);  //设置数字低通滤波器
    return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    u8 buf[2]; 
    short raw;
    float temp;
    MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    raw=((u16)buf[0]<<8)|buf[1];  
    temp=36.53+((double)raw)/340;  
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    u8 buf[6],res;  
    res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=((u16)buf[0]<<8)|buf[1];  
        *gy=((u16)buf[2]<<8)|buf[3];  
        *gz=((u16)buf[4]<<8)|buf[5];
    }   
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    u8 buf[6],res;  
    res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
    if(res==0)
    {
        *ax=((u16)buf[0]<<8)|buf[1];  
        *ay=((u16)buf[2]<<8)|buf[3];  
        *az=((u16)buf[4]<<8)|buf[5];
    }   
    return res;;
}
//IIC连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    u8 i; 
    MPU_IIC_Start(); 
    MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令 
    if(MPU_IIC_Wait_Ack())  //等待应答
    {
        MPU_IIC_Stop();      
        return 1;       
    }
    MPU_IIC_Send_Byte(reg); //写寄存器地址
    MPU_IIC_Wait_Ack();     //等待应答
    for(i=0;i<len;i++)
    {
        MPU_IIC_Send_Byte(buf[i]);  //发送数据
        if(MPU_IIC_Wait_Ack())      //等待ACK
        {
            MPU_IIC_Stop();  
            return 1;        
        }       
    }    
    MPU_IIC_Stop();  
    return 0;   
} 
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{ 
    MPU_IIC_Start(); 
    MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令 
    if(MPU_IIC_Wait_Ack())  //等待应答
    {
        MPU_IIC_Stop();      
        return 1;       
    }
    MPU_IIC_Send_Byte(reg); //写寄存器地址
    MPU_IIC_Wait_Ack();     //等待应答
    MPU_IIC_Start();
    MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令 
    MPU_IIC_Wait_Ack();     //等待应答 
    while(len)
    {
        if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK 
        else *buf=MPU_IIC_Read_Byte(1);     //读数据,发送ACK  
        len--;
        buf++; 
    }    
    MPU_IIC_Stop(); //产生一个停止条件 
    return 0;   
}
//IIC写一个字节 
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
u8 MPU_Write_Byte(u8 reg,u8 data)                
{ 
    MPU_IIC_Start(); 
    MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 
    if(MPU_IIC_Wait_Ack())  //等待应答
    {
        MPU_IIC_Stop();      
        return 1;       
    }
    MPU_IIC_Send_Byte(reg); //写寄存器地址
    MPU_IIC_Wait_Ack();     //等待应答 
    MPU_IIC_Send_Byte(data);//发送数据
    if(MPU_IIC_Wait_Ack())  //等待ACK
    {
        MPU_IIC_Stop();  
        return 1;        
    }        
    MPU_IIC_Stop();  
    return 0;
}
//IIC读一个字节 
//reg:寄存器地址 
//返回值:读到的数据
u8 MPU_Read_Byte(u8 reg)
{
    u8 res;
    MPU_IIC_Start(); 
    MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 
    MPU_IIC_Wait_Ack();     //等待应答 
    MPU_IIC_Send_Byte(reg); //写寄存器地址
    MPU_IIC_Wait_Ack();     //等待应答
    MPU_IIC_Start();
    MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令 
    MPU_IIC_Wait_Ack();     //等待应答 
    res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK 
    MPU_IIC_Stop();         //产生一个停止条件 
    return res;     
}

///////////////////////////////////////
//软件方法解算姿态
///////////////////////////////////////

/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：加速度、角速度
返回  值：无
**************************************************************************/

void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY)      
{
    X_Axis.angle+=(GyroX - X_Axis.Q_bias) * X_Axis.dt; //先验估计
    X_Axis.Pdot[0]=X_Axis.Q_angle - X_Axis.PP[0][1] - X_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分

    X_Axis.Pdot[1]=-X_Axis.PP[1][1];
    X_Axis.Pdot[2]=-X_Axis.PP[1][1];
    X_Axis.Pdot[3]=X_Axis.Q_gyro;
    X_Axis.PP[0][0] += X_Axis.Pdot[0] * X_Axis.dt;   // Pk-先验估计误差协方差微分的积分
    X_Axis.PP[0][1] += X_Axis.Pdot[1] * X_Axis.dt;   // =先验估计误差协方差
    X_Axis.PP[1][0] += X_Axis.Pdot[2] * X_Axis.dt;
    X_Axis.PP[1][1] += X_Axis.Pdot[3] * X_Axis.dt;
        
    X_Axis.Angle_err = AccelX - X_Axis.angle;  //zk-先验估计
    
    X_Axis.PCt_0 = X_Axis.C_0 * X_Axis.PP[0][0];
    X_Axis.PCt_1 = X_Axis.C_0 * X_Axis.PP[1][0];
    
    X_Axis.E = X_Axis.R_angle + X_Axis.C_0 * X_Axis.PCt_0;
    
    X_Axis.K_0 = X_Axis.PCt_0 / X_Axis.E;
    X_Axis.K_1 = X_Axis.PCt_1 / X_Axis.E;
    
    X_Axis.t_0 = X_Axis.PCt_0;
    X_Axis.t_1 = X_Axis.C_0 * X_Axis.PP[0][1];

    X_Axis.PP[0][0] -= X_Axis.K_0 * X_Axis.t_0;       //后验估计误差协方差
    X_Axis.PP[0][1] -= X_Axis.K_0 * X_Axis.t_1;
    X_Axis.PP[1][0] -= X_Axis.K_1 * X_Axis.t_0;
    X_Axis.PP[1][1] -= X_Axis.K_1 * X_Axis.t_1;
        
    X_Axis.angle   += X_Axis.K_0 * X_Axis.Angle_err;  //后验估计
    X_Axis.Q_bias  += X_Axis.K_1 * X_Axis.Angle_err;  //后验估计
    X_Axis.angle_dot   = GyroX - X_Axis.Q_bias;     //输出值(后验估计)的微分=角速度
    
    Y_Axis.angle+=(GyroY - Y_Axis.Q_bias) * Y_Axis.dt; //先验估计
    Y_Axis.Pdot[0]=Y_Axis.Q_angle - Y_Axis.PP[0][1] - Y_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分

    Y_Axis.Pdot[1]=-Y_Axis.PP[1][1];
    Y_Axis.Pdot[2]=-Y_Axis.PP[1][1];
    Y_Axis.Pdot[3]=Y_Axis.Q_gyro;
    Y_Axis.PP[0][0] += Y_Axis.Pdot[0] * Y_Axis.dt;   // Pk-先验估计误差协方差微分的积分
    Y_Axis.PP[0][1] += Y_Axis.Pdot[1] * Y_Axis.dt;   // =先验估计误差协方差
    Y_Axis.PP[1][0] += Y_Axis.Pdot[2] * Y_Axis.dt;
    Y_Axis.PP[1][1] += Y_Axis.Pdot[3] * Y_Axis.dt;
        
    Y_Axis.Angle_err = AccelY - Y_Axis.angle;  //zk-先验估计
    
    Y_Axis.PCt_0 = Y_Axis.C_0 * Y_Axis.PP[0][0];
    Y_Axis.PCt_1 = Y_Axis.C_0 * Y_Axis.PP[1][0];
    
    Y_Axis.E = Y_Axis.R_angle + Y_Axis.C_0 * Y_Axis.PCt_0;
    
    Y_Axis.K_0 = Y_Axis.PCt_0 / Y_Axis.E;
    Y_Axis.K_1 = Y_Axis.PCt_1 / Y_Axis.E;
    
    Y_Axis.t_0 = Y_Axis.PCt_0;
    Y_Axis.t_1 = Y_Axis.C_0 * Y_Axis.PP[0][1];

    Y_Axis.PP[0][0] -= Y_Axis.K_0 * Y_Axis.t_0;       //后验估计误差协方差
    Y_Axis.PP[0][1] -= Y_Axis.K_0 * Y_Axis.t_1;
    Y_Axis.PP[1][0] -= Y_Axis.K_1 * Y_Axis.t_0;
    Y_Axis.PP[1][1] -= Y_Axis.K_1 * Y_Axis.t_1;
        
    Y_Axis.angle   += Y_Axis.K_0 * Y_Axis.Angle_err;  //后验估计
    Y_Axis.Q_bias  += Y_Axis.K_1 * Y_Axis.Angle_err;  //后验估计
    Y_Axis.angle_dot   = GyroY - Y_Axis.Q_bias;     //输出值(后验估计)的微分=角速度
}

//////////////////////////////////////////////////////////////////////////////////   
//功能 : 主中断的服务函数
//参数 : 无
//返回值 : 无                              
//////////////////////////////////////////////////////////////////////////////////

void MainInterruptIRQ(void)
{
    float cpu_usage = 0;
    //系统监控部分代码
    TimeNow = SystemTimer.read_us();
    if(Flag_MeasureFreq)
    {
        FreqTime = TimeNow;
        FreqCounter = MainInterruptCounter;
        Flag_MeasureFreq = false;
    } 
    if((MainInterruptCounter - FreqCounter) == 100)
    {
        MainInterruptFreq = 100.0 * 1000000 / (TimeNow - FreqTime);
        Flag_MeasureFreq = true;
    }
    MainInterruptCounter++;
    
    
    //temp=MPU_Get_Temperature(); //得到温度值
    MPU_Get_Accelerometer(&aacx,&aacy,&aacz);   //得到加速度传感器数据
    MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //得到陀螺仪数据
    if(gyroy>32768)  gyroy-=65536;                       //数据类型转换  也可通过short强制类型转换
    if(gyroz>32768)  gyroz-=65536;                       //数据类型转换
    if(gyrox>32768)  gyrox-=65536;                       //数据类型转换
    if(aacx>32768) aacx-=65536;                      //数据类型转换
    if(aacz>32768) aacz-=65536;                      //数据类型转换
    if(aacy>32768) aacy-=65536;                      //数据类型转换

    ThetaXz=atan2((double)aacy,(double)aacz)*180/PI;  //重力加速度在XOZ、YOZ面上与z轴夹角
    ThetaYz=atan2((double)aacx,(double)aacz)*180/PI;                 //计算倾角   
    gyroy=gyroy/16.4;                                    //陀螺仪量程转换
    gyrox=gyrox/16.4;                                    //陀螺仪量程转换
    Kalman_Filter(ThetaXz,-gyrox, ThetaYz,-gyroy);

    Processing(); //用户进程
    cpu_usage = (float)(SystemTimer.read_us() - TimeNow) / 10000; //计算CPU使用率
    if(cpu_usage > 0)
        CpuUsage = cpu_usage;
}



//////////////////////////////////////////////////////////////////////////////////   
//功能 : 系统初始化
//参数 : 无
//返回值 : 无                              
//////////////////////////////////////////////////////////////////////////////////
void System_Init(void)
{
    SystemTimer.start();

    //TFT.DispInit();
    //TFT.FillScreen(ILI9340_BLACK);
    //TFT.SetRotation(3);
    //TFT.DrawString("System initing......", 50, 120, 3, ILI9340_WHITE);
    
    MPU_Init();
    MainInterrupt.attach(&MainInterruptIRQ, 0.01);
    
    Flag_MeasureFreq = true; //开始一次测量
    
    X_Axis.K1 = 0.02;
    X_Axis.Q_angle=0.001;
    X_Axis.Q_gyro=0.003;
    X_Axis.R_angle=0.5;
    X_Axis.dt=0.005;
    X_Axis.C_0 = 1;
    X_Axis.Pdot[0] = 0;
    X_Axis.Pdot[1] = 0;
    X_Axis.Pdot[2] = 0;
    X_Axis.Pdot[3] = 0;
    X_Axis.PP[0][0]=1;
    X_Axis.PP[0][1]=0;
    X_Axis.PP[1][0]=1;
    X_Axis.PP[1][1]=0;
    
    Y_Axis.K1 = 0.02;
    Y_Axis.Q_angle=0.001;
    Y_Axis.Q_gyro=0.003;
    Y_Axis.R_angle=0.5;
    Y_Axis.dt=0.005;
    Y_Axis.C_0 = 1;
    Y_Axis.Pdot[0] = 0;
    Y_Axis.Pdot[1] = 0;
    Y_Axis.Pdot[2] = 0;
    Y_Axis.Pdot[3] = 0;
    Y_Axis.PP[0][0]=1;
    Y_Axis.PP[0][1]=0;
    Y_Axis.PP[1][0]=1;
    Y_Axis.PP[1][1]=0;
}

//此处--推荐--配置外设、定义全局变量
//-------------------代码块开始

//DigitalOut myled(LED1);
AnalogIn LeftJoystickX(Pin_LeftJoystickX);
AnalogIn LeftJoystickY(Pin_LeftJoystickY);
AnalogIn RightJoystickX(Pin_RightJoystickX);
AnalogIn RightJoystickY(Pin_RightJoystickY);

float LeftJoystickValueX = 0.0, LeftJoystickValueY = 0.0, RightJoystickValueX = 0.0, RightJoystickValueY = 0.0;  //摇杆x,y轴的读数 归一化到 0-1

Servo Servo1(Pin_Servo1);
Servo Servo2(Pin_Servo2);
Servo Servo3(Pin_Servo3);
Servo Servo4(Pin_Servo4);
Servo Servo5(Pin_Servo5);
Servo Servo6(Pin_Servo6);
Servo Servo7(Pin_Servo7);
Servo Servo8(Pin_Servo8);
Servo Servo9(Pin_Servo9);
Servo Servo10(Pin_Servo10);
Servo Servo11(Pin_Servo11);
Servo Servo12(Pin_Servo12);

Serial UserUart(PC_10, PC_11);


//-------------------代码块结束

    int i = 0, j;
        float degree1 =  45.0, degree2 = 135.0;
//////////////////////////////////////////////////////////////////////////////////   
//功能 : 用户进程函数 用于执行用户自定义的后台任务 执行频率是100Hz
//参数 : 无
//返回值 : 无                              
//////////////////////////////////////////////////////////////////////////////////
void Processing(void)
{
    LeftJoystickValueX = LeftJoystickX.read();
    LeftJoystickValueY = LeftJoystickY.read();
    RightJoystickValueX = RightJoystickX.read();
    RightJoystickValueY = RightJoystickY.read();
    if((MainInterruptCounter % 50) == 0)
        {
            if(i == 0)
            {
                i = !i;
                Servo1.SetDegree(degree1);
                Servo2.SetDegree(degree1);
                Servo3.SetDegree(degree1);
                Servo4.SetDegree(degree1);
                Servo5.SetDegree(degree1);
                Servo6.SetDegree(degree1);
                Servo7.SetDegree(degree1);
                Servo8.SetDegree(degree1);
                Servo9.SetDegree(degree1);
                Servo10.SetDegree(degree1);
                Servo11.SetDegree(degree1);
                Servo12.SetDegree(degree1);
            }
            else
            {
                i = !i;
                Servo1.SetDegree(degree2);
                Servo2.SetDegree(degree2);
                Servo3.SetDegree(degree2);
                Servo4.SetDegree(degree2);
                Servo5.SetDegree(degree2);
                Servo6.SetDegree(degree2);
                Servo7.SetDegree(degree2);
                Servo8.SetDegree(degree2);
                Servo9.SetDegree(degree2);
                Servo10.SetDegree(degree2);
                Servo11.SetDegree(degree2);
                Servo12.SetDegree(degree2);
            }
        }
}
 
int main() 
{
    ILI9340_Display TFT = ILI9340_Display(Pin_TFT_MOSI, Pin_TFT_MISO, Pin_TFT_SCLK, Pin_TFT_CS, Pin_TFT_RESET, Pin_TFT_DC); //液晶屏初始化 已知bug 液晶屏类的定义必须在所有DigitalOut类的定义之后否则无法正常工作
    TFT_LED = 1; //点亮TFT背光
    TFT.DispInit();
    TFT.FillScreen(ILI9340_BLACK);
    TFT.SetRotation(3);
    TFT.DrawString("iCenter", 76, 86, 3, ILI9340_WHITE);
    TFT.DrawString("System initing...", 24, 146, 2, ILI9340_WHITE);
    System_Init();
    TFT.DrawString("iCenter", 76, 86, 3, ILI9340_BLACK);
    TFT.DrawString("System initing...", 24, 146, 2, ILI9340_BLACK);
    
    while(true) {    
        printf("CPU Usage : %.2f\r\n", CpuUsage);
        printf("Pitch : %.2f\r\n", Y_Axis.angle);
        printf("Roll : %.2f\r\n", X_Axis.angle);
        printf("LeftJoystickValueX : %.2f\r\n", LeftJoystickValueX);
        printf("LeftJoystickValueY : %.2f\r\n", LeftJoystickValueY);
        printf("RightJoystickValueX : %.2f\r\n", RightJoystickValueX);
        printf("RightJoystickValueY : %.2f\r\n", RightJoystickValueY);
        
        // Small amount of text to the display.
        
        
        // Do the waiting thang...
        wait(0.5);

    }
}