Chen Huan / Mbed 2 deprecated Final_DongGanPingTai

Tsinghua Icenter ChenHuan

Dependencies:   mbed

main.cpp@0:9b8df4f9b792, 2017-03-16 (annotated)

Committer:
heroistired
Date:
Thu Mar 16 13:07:14 2017 +0000
Revision:
0:9b8df4f9b792
DongGanPingTai

Who changed what in which revision?

UserRevisionLine numberNew contents of line
heroistired 0:9b8df4f9b792 1 /***************************************************************
heroistired 0:9b8df4f9b792 2 功能 : 基于mbed的动感平台SDK 包含摇杆 位置传感器 舵机 以及液晶屏等外设
heroistired 0:9b8df4f9b792 3 作者 : 陈欢 清华大学电机工程与应用电子技术系
heroistired 0:9b8df4f9b792 4 邮箱 : h-che14@mails.tsinghua.edu.cn OR heroistired@gmail.com
heroistired 0:9b8df4f9b792 5 声明 :
heroistired 0:9b8df4f9b792 6 本程序仅供学习与交流使用,如需他用,须联系作者
heroistired 0:9b8df4f9b792 7 All rights reserved
heroistired 0:9b8df4f9b792 8 2017.2.15
heroistired 0:9b8df4f9b792 9 ***************************************************************/
heroistired 0:9b8df4f9b792 10
heroistired 0:9b8df4f9b792 11 #include "mbed.h"
heroistired 0:9b8df4f9b792 12 #include "mpu6050_config.h"
heroistired 0:9b8df4f9b792 13 #include "Servo.h"
heroistired 0:9b8df4f9b792 14 #include "TFT_ILI9340.h"
heroistired 0:9b8df4f9b792 15 #include "PinMap.h"
heroistired 0:9b8df4f9b792 16 Serial SystemUart(SERIAL_TX, SERIAL_RX); //系统串口初始化
heroistired 0:9b8df4f9b792 17
heroistired 0:9b8df4f9b792 18 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 19 //头文件部分
heroistired 0:9b8df4f9b792 20 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 21
heroistired 0:9b8df4f9b792 22 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 23 //系统相关
heroistired 0:9b8df4f9b792 24 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 25
heroistired 0:9b8df4f9b792 26 //系统监控
heroistired 0:9b8df4f9b792 27 //接受外部调用的变量
heroistired 0:9b8df4f9b792 28 int TimeNow = 0; //当前的系统时间
heroistired 0:9b8df4f9b792 29 float CpuUsage = 0.0; //当前的CPU使用率 以主中断为基准
heroistired 0:9b8df4f9b792 30 unsigned int MainInterruptCounter = 0; //存储系统主中断的总执行次数
heroistired 0:9b8df4f9b792 31 float MainInterruptFreq = 0.0; //存储系统主中断的实际频率 Hz
heroistired 0:9b8df4f9b792 32 //中间变量 不接受外部调用
heroistired 0:9b8df4f9b792 33 int FreqTime = 0; //计算系统中断的实际频率 存储的是测量开始时的系统时间
heroistired 0:9b8df4f9b792 34 int FreqCounter = 0; //计算系统中断的实际频率 存储的是测量开始时的中断执行次数
heroistired 0:9b8df4f9b792 35 bool Flag_MeasureFreq = true; //标志 是否要进行一次频率测量
heroistired 0:9b8df4f9b792 36
heroistired 0:9b8df4f9b792 37
heroistired 0:9b8df4f9b792 38 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 39 //mpuiic.h
heroistired 0:9b8df4f9b792 40 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 41
heroistired 0:9b8df4f9b792 42 //声明IIC需要用到的IO口
heroistired 0:9b8df4f9b792 43 DigitalOut MPU_IIC_SCL(Pin_MPU6050SCL); //SCL
heroistired 0:9b8df4f9b792 44 DigitalInOut MPU_IIC_SDA(Pin_MPU6050SDA); //SDA
heroistired 0:9b8df4f9b792 45
heroistired 0:9b8df4f9b792 46 //弥补屏幕LED画错的bug
heroistired 0:9b8df4f9b792 47 DigitalOut TFT_LED(Pin_TFT_LED);
heroistired 0:9b8df4f9b792 48
heroistired 0:9b8df4f9b792 49
heroistired 0:9b8df4f9b792 50 //定义IO操作的宏
heroistired 0:9b8df4f9b792 51 #define MPU_SDA_IN() MPU_IIC_SDA.input()
heroistired 0:9b8df4f9b792 52 #define MPU_SDA_OUT() MPU_IIC_SDA.output()
heroistired 0:9b8df4f9b792 53 #define MPU_READ_SDA MPU_IIC_SDA.read() //读取SDA
heroistired 0:9b8df4f9b792 54
heroistired 0:9b8df4f9b792 55 //兼容性宏定义
heroistired 0:9b8df4f9b792 56 #define delay_us wait_us
heroistired 0:9b8df4f9b792 57 #define delay_ms wait_ms
heroistired 0:9b8df4f9b792 58 #define u8 unsigned char
heroistired 0:9b8df4f9b792 59 #define u16 unsigned short
heroistired 0:9b8df4f9b792 60
heroistired 0:9b8df4f9b792 61 //IIC所有操作函数
heroistired 0:9b8df4f9b792 62 void MPU_IIC_Delay(void); //MPU IIC延时函数
heroistired 0:9b8df4f9b792 63 void MPU_IIC_Init(void); //初始化IIC的IO口
heroistired 0:9b8df4f9b792 64 void MPU_IIC_Start(void); //发送IIC开始信号
heroistired 0:9b8df4f9b792 65 void MPU_IIC_Stop(void); //发送IIC停止信号
heroistired 0:9b8df4f9b792 66 void MPU_IIC_Send_Byte(u8 txd); //IIC发送一个字节
heroistired 0:9b8df4f9b792 67 u8 MPU_IIC_Read_Byte(unsigned char ack);//IIC读取一个字节
heroistired 0:9b8df4f9b792 68 u8 MPU_IIC_Wait_Ack(void); //IIC等待ACK信号
heroistired 0:9b8df4f9b792 69 void MPU_IIC_Ack(void); //IIC发送ACK信号
heroistired 0:9b8df4f9b792 70 void MPU_IIC_NAck(void); //IIC不发送ACK信号
heroistired 0:9b8df4f9b792 71
heroistired 0:9b8df4f9b792 72 void IMPU_IC_Write_One_Byte(u8 daddr,u8 addr,u8 data);
heroistired 0:9b8df4f9b792 73 u8 MPU_IIC_Read_One_Byte(u8 daddr,u8 addr);
heroistired 0:9b8df4f9b792 74
heroistired 0:9b8df4f9b792 75 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 76 //mp6050.h
heroistired 0:9b8df4f9b792 77 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 78 u8 MPU_Init(void); //初始化MPU6050
heroistired 0:9b8df4f9b792 79 u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写
heroistired 0:9b8df4f9b792 80 u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读
heroistired 0:9b8df4f9b792 81 u8 MPU_Write_Byte(u8 reg,u8 data); //IIC写一个字节
heroistired 0:9b8df4f9b792 82 u8 MPU_Read_Byte(u8 reg); //IIC读一个字节
heroistired 0:9b8df4f9b792 83
heroistired 0:9b8df4f9b792 84 u8 MPU_Set_Gyro_Fsr(u8 fsr);
heroistired 0:9b8df4f9b792 85 u8 MPU_Set_Accel_Fsr(u8 fsr);
heroistired 0:9b8df4f9b792 86 u8 MPU_Set_LPF(u16 lpf);
heroistired 0:9b8df4f9b792 87 u8 MPU_Set_Rate(u16 rate);
heroistired 0:9b8df4f9b792 88 u8 MPU_Set_Fifo(u8 sens);
heroistired 0:9b8df4f9b792 89
heroistired 0:9b8df4f9b792 90
heroistired 0:9b8df4f9b792 91 short MPU_Get_Temperature(void);
heroistired 0:9b8df4f9b792 92 u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
heroistired 0:9b8df4f9b792 93 u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
heroistired 0:9b8df4f9b792 94
heroistired 0:9b8df4f9b792 95 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 96 //软件方法解算姿态
heroistired 0:9b8df4f9b792 97 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 98 Timer SystemTimer; //系统计时器
heroistired 0:9b8df4f9b792 99 Ticker MainInterrupt; //主中断 100Hz
heroistired 0:9b8df4f9b792 100 //加速度 角速度 温度(原始值)
heroistired 0:9b8df4f9b792 101 short aacx = 0, aacy = 0, aacz = 0, gyrox = 0, gyroy = 0, gyroz = 0, temp = 0;
heroistired 0:9b8df4f9b792 102 float Pitch = 0, Yaw = 0, Roll = 0; //绕x,y,z轴的转角
heroistired 0:9b8df4f9b792 103 float ThetaXz = 0, ThetaYz = 0; //重力加速度在XOZ、YOZ面上与z轴夹角
heroistired 0:9b8df4f9b792 104
heroistired 0:9b8df4f9b792 105 //卡尔曼滤波器相关参量
heroistired 0:9b8df4f9b792 106 #define PI 3.1415926
heroistired 0:9b8df4f9b792 107 struct KalmanStruct
heroistired 0:9b8df4f9b792 108 {
heroistired 0:9b8df4f9b792 109 float K1;
heroistired 0:9b8df4f9b792 110 float angle, angle_dot;
heroistired 0:9b8df4f9b792 111 float Q_angle;// 过程噪声的协方差
heroistired 0:9b8df4f9b792 112 float Q_gyro;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
heroistired 0:9b8df4f9b792 113 float R_angle;// 测量噪声的协方差 既测量偏差
heroistired 0:9b8df4f9b792 114 float dt;//
heroistired 0:9b8df4f9b792 115 char C_0;
heroistired 0:9b8df4f9b792 116 float Q_bias, Angle_err;
heroistired 0:9b8df4f9b792 117 float PCt_0, PCt_1, E;
heroistired 0:9b8df4f9b792 118 float K_0, K_1, t_0, t_1;
heroistired 0:9b8df4f9b792 119 float Pdot[4];
heroistired 0:9b8df4f9b792 120 float PP[2][2];
heroistired 0:9b8df4f9b792 121 };
heroistired 0:9b8df4f9b792 122
heroistired 0:9b8df4f9b792 123 struct KalmanStruct X_Axis = {0}, Y_Axis = {0};
heroistired 0:9b8df4f9b792 124
heroistired 0:9b8df4f9b792 125 void MainInterruptIRQ(void); //主中断服务函数
heroistired 0:9b8df4f9b792 126 void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY); //卡尔曼滤波器
heroistired 0:9b8df4f9b792 127 void Processing(void); //用户进程函数
heroistired 0:9b8df4f9b792 128
heroistired 0:9b8df4f9b792 129 //系统初始化函数
heroistired 0:9b8df4f9b792 130 void System_Init(void);
heroistired 0:9b8df4f9b792 131
heroistired 0:9b8df4f9b792 132 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 133 //函数实现部分
heroistired 0:9b8df4f9b792 134 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 135
heroistired 0:9b8df4f9b792 136 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 137 //mpuiic.c
heroistired 0:9b8df4f9b792 138 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 139
heroistired 0:9b8df4f9b792 140 //MPU IIC 延时函数
heroistired 0:9b8df4f9b792 141 void MPU_IIC_Delay(void)
heroistired 0:9b8df4f9b792 142 {
heroistired 0:9b8df4f9b792 143 delay_us(2);
heroistired 0:9b8df4f9b792 144 }
heroistired 0:9b8df4f9b792 145 //初始化IIC
heroistired 0:9b8df4f9b792 146 void MPU_IIC_Init(void)
heroistired 0:9b8df4f9b792 147 {
heroistired 0:9b8df4f9b792 148 RCC->APB2ENR|=1<<4; //先使能外设IO PORTC时钟
heroistired 0:9b8df4f9b792 149 GPIOC->CRH&=0XFFF00FFF; //PC11/12 推挽输出
heroistired 0:9b8df4f9b792 150 GPIOC->CRH|=0X00033000;
heroistired 0:9b8df4f9b792 151 GPIOC->ODR|=3<<11; //PC11,12 输出高
heroistired 0:9b8df4f9b792 152 }
heroistired 0:9b8df4f9b792 153 //产生IIC起始信号
heroistired 0:9b8df4f9b792 154 void MPU_IIC_Start(void)
heroistired 0:9b8df4f9b792 155 {
heroistired 0:9b8df4f9b792 156 MPU_SDA_OUT(); //sda线输出
heroistired 0:9b8df4f9b792 157 MPU_IIC_SDA=1;
heroistired 0:9b8df4f9b792 158 MPU_IIC_SCL=1;
heroistired 0:9b8df4f9b792 159 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 160 MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low
heroistired 0:9b8df4f9b792 161 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 162 MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据
heroistired 0:9b8df4f9b792 163 }
heroistired 0:9b8df4f9b792 164 //产生IIC停止信号
heroistired 0:9b8df4f9b792 165 void MPU_IIC_Stop(void)
heroistired 0:9b8df4f9b792 166 {
heroistired 0:9b8df4f9b792 167 MPU_SDA_OUT();//sda线输出
heroistired 0:9b8df4f9b792 168 MPU_IIC_SCL=0;
heroistired 0:9b8df4f9b792 169 MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
heroistired 0:9b8df4f9b792 170 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 171 MPU_IIC_SCL=1;
heroistired 0:9b8df4f9b792 172 MPU_IIC_SDA=1;//发送I2C总线结束信号
heroistired 0:9b8df4f9b792 173 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 174 }
heroistired 0:9b8df4f9b792 175 //等待应答信号到来
heroistired 0:9b8df4f9b792 176 //返回值：1，接收应答失败
heroistired 0:9b8df4f9b792 177 // 0，接收应答成功
heroistired 0:9b8df4f9b792 178 u8 MPU_IIC_Wait_Ack(void)
heroistired 0:9b8df4f9b792 179 {
heroistired 0:9b8df4f9b792 180 u8 ucErrTime=0;
heroistired 0:9b8df4f9b792 181 MPU_SDA_IN(); //SDA设置为输入
heroistired 0:9b8df4f9b792 182 MPU_IIC_SDA=1;MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 183 MPU_IIC_SCL=1;MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 184 while(MPU_READ_SDA)
heroistired 0:9b8df4f9b792 185 {
heroistired 0:9b8df4f9b792 186 ucErrTime++;
heroistired 0:9b8df4f9b792 187 if(ucErrTime>250)
heroistired 0:9b8df4f9b792 188 {
heroistired 0:9b8df4f9b792 189 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 190 return 1;
heroistired 0:9b8df4f9b792 191 }
heroistired 0:9b8df4f9b792 192 }
heroistired 0:9b8df4f9b792 193 MPU_IIC_SCL=0;//时钟输出0
heroistired 0:9b8df4f9b792 194 return 0;
heroistired 0:9b8df4f9b792 195 }
heroistired 0:9b8df4f9b792 196 //产生ACK应答
heroistired 0:9b8df4f9b792 197 void MPU_IIC_Ack(void)
heroistired 0:9b8df4f9b792 198 {
heroistired 0:9b8df4f9b792 199 MPU_IIC_SCL=0;
heroistired 0:9b8df4f9b792 200 MPU_SDA_OUT();
heroistired 0:9b8df4f9b792 201 MPU_IIC_SDA=0;
heroistired 0:9b8df4f9b792 202 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 203 MPU_IIC_SCL=1;
heroistired 0:9b8df4f9b792 204 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 205 MPU_IIC_SCL=0;
heroistired 0:9b8df4f9b792 206 }
heroistired 0:9b8df4f9b792 207 //不产生ACK应答
heroistired 0:9b8df4f9b792 208 void MPU_IIC_NAck(void)
heroistired 0:9b8df4f9b792 209 {
heroistired 0:9b8df4f9b792 210 MPU_IIC_SCL=0;
heroistired 0:9b8df4f9b792 211 MPU_SDA_OUT();
heroistired 0:9b8df4f9b792 212 MPU_IIC_SDA=1;
heroistired 0:9b8df4f9b792 213 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 214 MPU_IIC_SCL=1;
heroistired 0:9b8df4f9b792 215 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 216 MPU_IIC_SCL=0;
heroistired 0:9b8df4f9b792 217 }
heroistired 0:9b8df4f9b792 218 //IIC发送一个字节
heroistired 0:9b8df4f9b792 219 //返回从机有无应答
heroistired 0:9b8df4f9b792 220 //1，有应答
heroistired 0:9b8df4f9b792 221 //0，无应答
heroistired 0:9b8df4f9b792 222 void MPU_IIC_Send_Byte(u8 txd)
heroistired 0:9b8df4f9b792 223 {
heroistired 0:9b8df4f9b792 224 u8 t;
heroistired 0:9b8df4f9b792 225 MPU_SDA_OUT();
heroistired 0:9b8df4f9b792 226 MPU_IIC_SCL=0;//拉低时钟开始数据传输
heroistired 0:9b8df4f9b792 227 for(t=0;t<8;t++)
heroistired 0:9b8df4f9b792 228 {
heroistired 0:9b8df4f9b792 229 MPU_IIC_SDA=(txd&0x80)>>7;
heroistired 0:9b8df4f9b792 230 txd<<=1;
heroistired 0:9b8df4f9b792 231 MPU_IIC_SCL=1;
heroistired 0:9b8df4f9b792 232 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 233 MPU_IIC_SCL=0;
heroistired 0:9b8df4f9b792 234 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 235 }
heroistired 0:9b8df4f9b792 236 }
heroistired 0:9b8df4f9b792 237 //读1个字节，ack=1时，发送ACK，ack=0，发送nACK
heroistired 0:9b8df4f9b792 238 u8 MPU_IIC_Read_Byte(unsigned char ack)
heroistired 0:9b8df4f9b792 239 {
heroistired 0:9b8df4f9b792 240 unsigned char i,receive=0;
heroistired 0:9b8df4f9b792 241 MPU_SDA_IN();//SDA设置为输入
heroistired 0:9b8df4f9b792 242 for(i=0;i<8;i++ )
heroistired 0:9b8df4f9b792 243 {
heroistired 0:9b8df4f9b792 244 MPU_IIC_SCL=0;
heroistired 0:9b8df4f9b792 245 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 246 MPU_IIC_SCL=1;
heroistired 0:9b8df4f9b792 247 receive<<=1;
heroistired 0:9b8df4f9b792 248 if(MPU_READ_SDA)receive++;
heroistired 0:9b8df4f9b792 249 MPU_IIC_Delay();
heroistired 0:9b8df4f9b792 250 }
heroistired 0:9b8df4f9b792 251 if (!ack)
heroistired 0:9b8df4f9b792 252 MPU_IIC_NAck();//发送nACK
heroistired 0:9b8df4f9b792 253 else
heroistired 0:9b8df4f9b792 254 MPU_IIC_Ack(); //发送ACK
heroistired 0:9b8df4f9b792 255 return receive;
heroistired 0:9b8df4f9b792 256 }
heroistired 0:9b8df4f9b792 257
heroistired 0:9b8df4f9b792 258 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 259 //mp6050.c
heroistired 0:9b8df4f9b792 260 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 261 //初始化MPU6050
heroistired 0:9b8df4f9b792 262 //返回值:0,成功
heroistired 0:9b8df4f9b792 263 // 其他,错误代码
heroistired 0:9b8df4f9b792 264 u8 MPU_Init(void)
heroistired 0:9b8df4f9b792 265 {
heroistired 0:9b8df4f9b792 266 u8 res;
heroistired 0:9b8df4f9b792 267 MPU_IIC_Init();//初始化IIC总线
heroistired 0:9b8df4f9b792 268 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
heroistired 0:9b8df4f9b792 269 delay_ms(100);
heroistired 0:9b8df4f9b792 270 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
heroistired 0:9b8df4f9b792 271 MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
heroistired 0:9b8df4f9b792 272 MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
heroistired 0:9b8df4f9b792 273 MPU_Set_Rate(200); //设置采样率200Hz
heroistired 0:9b8df4f9b792 274 MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
heroistired 0:9b8df4f9b792 275 MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
heroistired 0:9b8df4f9b792 276 MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
heroistired 0:9b8df4f9b792 277 MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
heroistired 0:9b8df4f9b792 278 res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
heroistired 0:9b8df4f9b792 279 if(res==MPU_ADDR)//器件ID正确
heroistired 0:9b8df4f9b792 280 {
heroistired 0:9b8df4f9b792 281 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
heroistired 0:9b8df4f9b792 282 MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
heroistired 0:9b8df4f9b792 283 MPU_Set_Rate(200); //设置采样率为200Hz
heroistired 0:9b8df4f9b792 284 }else return 1;
heroistired 0:9b8df4f9b792 285 return 0;
heroistired 0:9b8df4f9b792 286 }
heroistired 0:9b8df4f9b792 287 //设置MPU6050陀螺仪传感器满量程范围
heroistired 0:9b8df4f9b792 288 //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
heroistired 0:9b8df4f9b792 289 //返回值:0,设置成功
heroistired 0:9b8df4f9b792 290 // 其他,设置失败
heroistired 0:9b8df4f9b792 291 u8 MPU_Set_Gyro_Fsr(u8 fsr)
heroistired 0:9b8df4f9b792 292 {
heroistired 0:9b8df4f9b792 293 return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
heroistired 0:9b8df4f9b792 294 }
heroistired 0:9b8df4f9b792 295 //设置MPU6050加速度传感器满量程范围
heroistired 0:9b8df4f9b792 296 //fsr:0,±2g;1,±4g;2,±8g;3,±16g
heroistired 0:9b8df4f9b792 297 //返回值:0,设置成功
heroistired 0:9b8df4f9b792 298 // 其他,设置失败
heroistired 0:9b8df4f9b792 299 u8 MPU_Set_Accel_Fsr(u8 fsr)
heroistired 0:9b8df4f9b792 300 {
heroistired 0:9b8df4f9b792 301 return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
heroistired 0:9b8df4f9b792 302 }
heroistired 0:9b8df4f9b792 303 //设置MPU6050的数字低通滤波器
heroistired 0:9b8df4f9b792 304 //lpf:数字低通滤波频率(Hz)
heroistired 0:9b8df4f9b792 305 //返回值:0,设置成功
heroistired 0:9b8df4f9b792 306 // 其他,设置失败
heroistired 0:9b8df4f9b792 307 u8 MPU_Set_LPF(u16 lpf)
heroistired 0:9b8df4f9b792 308 {
heroistired 0:9b8df4f9b792 309 u8 data=0;
heroistired 0:9b8df4f9b792 310 if(lpf>=188)data=1;
heroistired 0:9b8df4f9b792 311 else if(lpf>=98)data=2;
heroistired 0:9b8df4f9b792 312 else if(lpf>=42)data=3;
heroistired 0:9b8df4f9b792 313 else if(lpf>=20)data=4;
heroistired 0:9b8df4f9b792 314 else if(lpf>=10)data=5;
heroistired 0:9b8df4f9b792 315 else data=6;
heroistired 0:9b8df4f9b792 316 return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
heroistired 0:9b8df4f9b792 317 }
heroistired 0:9b8df4f9b792 318 //设置MPU6050的采样率(假定Fs=1KHz)
heroistired 0:9b8df4f9b792 319 //rate:4~1000(Hz)
heroistired 0:9b8df4f9b792 320 //返回值:0,设置成功
heroistired 0:9b8df4f9b792 321 // 其他,设置失败
heroistired 0:9b8df4f9b792 322 u8 MPU_Set_Rate(u16 rate)
heroistired 0:9b8df4f9b792 323 {
heroistired 0:9b8df4f9b792 324 u8 data;
heroistired 0:9b8df4f9b792 325 if(rate>1000)rate=1000;
heroistired 0:9b8df4f9b792 326 if(rate<4)rate=4;
heroistired 0:9b8df4f9b792 327 data=1000/rate-1;
heroistired 0:9b8df4f9b792 328 data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
heroistired 0:9b8df4f9b792 329 return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
heroistired 0:9b8df4f9b792 330 }
heroistired 0:9b8df4f9b792 331
heroistired 0:9b8df4f9b792 332 //得到温度值
heroistired 0:9b8df4f9b792 333 //返回值:温度值(扩大了100倍)
heroistired 0:9b8df4f9b792 334 short MPU_Get_Temperature(void)
heroistired 0:9b8df4f9b792 335 {
heroistired 0:9b8df4f9b792 336 u8 buf[2];
heroistired 0:9b8df4f9b792 337 short raw;
heroistired 0:9b8df4f9b792 338 float temp;
heroistired 0:9b8df4f9b792 339 MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
heroistired 0:9b8df4f9b792 340 raw=((u16)buf[0]<<8)|buf[1];
heroistired 0:9b8df4f9b792 341 temp=36.53+((double)raw)/340;
heroistired 0:9b8df4f9b792 342 return temp*100;;
heroistired 0:9b8df4f9b792 343 }
heroistired 0:9b8df4f9b792 344 //得到陀螺仪值(原始值)
heroistired 0:9b8df4f9b792 345 //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
heroistired 0:9b8df4f9b792 346 //返回值:0,成功
heroistired 0:9b8df4f9b792 347 // 其他,错误代码
heroistired 0:9b8df4f9b792 348 u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
heroistired 0:9b8df4f9b792 349 {
heroistired 0:9b8df4f9b792 350 u8 buf[6],res;
heroistired 0:9b8df4f9b792 351 res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
heroistired 0:9b8df4f9b792 352 if(res==0)
heroistired 0:9b8df4f9b792 353 {
heroistired 0:9b8df4f9b792 354 *gx=((u16)buf[0]<<8)|buf[1];
heroistired 0:9b8df4f9b792 355 *gy=((u16)buf[2]<<8)|buf[3];
heroistired 0:9b8df4f9b792 356 *gz=((u16)buf[4]<<8)|buf[5];
heroistired 0:9b8df4f9b792 357 }
heroistired 0:9b8df4f9b792 358 return res;;
heroistired 0:9b8df4f9b792 359 }
heroistired 0:9b8df4f9b792 360 //得到加速度值(原始值)
heroistired 0:9b8df4f9b792 361 //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
heroistired 0:9b8df4f9b792 362 //返回值:0,成功
heroistired 0:9b8df4f9b792 363 // 其他,错误代码
heroistired 0:9b8df4f9b792 364 u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
heroistired 0:9b8df4f9b792 365 {
heroistired 0:9b8df4f9b792 366 u8 buf[6],res;
heroistired 0:9b8df4f9b792 367 res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
heroistired 0:9b8df4f9b792 368 if(res==0)
heroistired 0:9b8df4f9b792 369 {
heroistired 0:9b8df4f9b792 370 *ax=((u16)buf[0]<<8)|buf[1];
heroistired 0:9b8df4f9b792 371 *ay=((u16)buf[2]<<8)|buf[3];
heroistired 0:9b8df4f9b792 372 *az=((u16)buf[4]<<8)|buf[5];
heroistired 0:9b8df4f9b792 373 }
heroistired 0:9b8df4f9b792 374 return res;;
heroistired 0:9b8df4f9b792 375 }
heroistired 0:9b8df4f9b792 376 //IIC连续写
heroistired 0:9b8df4f9b792 377 //addr:器件地址
heroistired 0:9b8df4f9b792 378 //reg:寄存器地址
heroistired 0:9b8df4f9b792 379 //len:写入长度
heroistired 0:9b8df4f9b792 380 //buf:数据区
heroistired 0:9b8df4f9b792 381 //返回值:0,正常
heroistired 0:9b8df4f9b792 382 // 其他,错误代码
heroistired 0:9b8df4f9b792 383 u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
heroistired 0:9b8df4f9b792 384 {
heroistired 0:9b8df4f9b792 385 u8 i;
heroistired 0:9b8df4f9b792 386 MPU_IIC_Start();
heroistired 0:9b8df4f9b792 387 MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
heroistired 0:9b8df4f9b792 388 if(MPU_IIC_Wait_Ack()) //等待应答
heroistired 0:9b8df4f9b792 389 {
heroistired 0:9b8df4f9b792 390 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 391 return 1;
heroistired 0:9b8df4f9b792 392 }
heroistired 0:9b8df4f9b792 393 MPU_IIC_Send_Byte(reg); //写寄存器地址
heroistired 0:9b8df4f9b792 394 MPU_IIC_Wait_Ack(); //等待应答
heroistired 0:9b8df4f9b792 395 for(i=0;i<len;i++)
heroistired 0:9b8df4f9b792 396 {
heroistired 0:9b8df4f9b792 397 MPU_IIC_Send_Byte(buf[i]); //发送数据
heroistired 0:9b8df4f9b792 398 if(MPU_IIC_Wait_Ack()) //等待ACK
heroistired 0:9b8df4f9b792 399 {
heroistired 0:9b8df4f9b792 400 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 401 return 1;
heroistired 0:9b8df4f9b792 402 }
heroistired 0:9b8df4f9b792 403 }
heroistired 0:9b8df4f9b792 404 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 405 return 0;
heroistired 0:9b8df4f9b792 406 }
heroistired 0:9b8df4f9b792 407 //IIC连续读
heroistired 0:9b8df4f9b792 408 //addr:器件地址
heroistired 0:9b8df4f9b792 409 //reg:要读取的寄存器地址
heroistired 0:9b8df4f9b792 410 //len:要读取的长度
heroistired 0:9b8df4f9b792 411 //buf:读取到的数据存储区
heroistired 0:9b8df4f9b792 412 //返回值:0,正常
heroistired 0:9b8df4f9b792 413 // 其他,错误代码
heroistired 0:9b8df4f9b792 414 u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
heroistired 0:9b8df4f9b792 415 {
heroistired 0:9b8df4f9b792 416 MPU_IIC_Start();
heroistired 0:9b8df4f9b792 417 MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
heroistired 0:9b8df4f9b792 418 if(MPU_IIC_Wait_Ack()) //等待应答
heroistired 0:9b8df4f9b792 419 {
heroistired 0:9b8df4f9b792 420 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 421 return 1;
heroistired 0:9b8df4f9b792 422 }
heroistired 0:9b8df4f9b792 423 MPU_IIC_Send_Byte(reg); //写寄存器地址
heroistired 0:9b8df4f9b792 424 MPU_IIC_Wait_Ack(); //等待应答
heroistired 0:9b8df4f9b792 425 MPU_IIC_Start();
heroistired 0:9b8df4f9b792 426 MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
heroistired 0:9b8df4f9b792 427 MPU_IIC_Wait_Ack(); //等待应答
heroistired 0:9b8df4f9b792 428 while(len)
heroistired 0:9b8df4f9b792 429 {
heroistired 0:9b8df4f9b792 430 if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK
heroistired 0:9b8df4f9b792 431 else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
heroistired 0:9b8df4f9b792 432 len--;
heroistired 0:9b8df4f9b792 433 buf++;
heroistired 0:9b8df4f9b792 434 }
heroistired 0:9b8df4f9b792 435 MPU_IIC_Stop(); //产生一个停止条件
heroistired 0:9b8df4f9b792 436 return 0;
heroistired 0:9b8df4f9b792 437 }
heroistired 0:9b8df4f9b792 438 //IIC写一个字节
heroistired 0:9b8df4f9b792 439 //reg:寄存器地址
heroistired 0:9b8df4f9b792 440 //data:数据
heroistired 0:9b8df4f9b792 441 //返回值:0,正常
heroistired 0:9b8df4f9b792 442 // 其他,错误代码
heroistired 0:9b8df4f9b792 443 u8 MPU_Write_Byte(u8 reg,u8 data)
heroistired 0:9b8df4f9b792 444 {
heroistired 0:9b8df4f9b792 445 MPU_IIC_Start();
heroistired 0:9b8df4f9b792 446 MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
heroistired 0:9b8df4f9b792 447 if(MPU_IIC_Wait_Ack()) //等待应答
heroistired 0:9b8df4f9b792 448 {
heroistired 0:9b8df4f9b792 449 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 450 return 1;
heroistired 0:9b8df4f9b792 451 }
heroistired 0:9b8df4f9b792 452 MPU_IIC_Send_Byte(reg); //写寄存器地址
heroistired 0:9b8df4f9b792 453 MPU_IIC_Wait_Ack(); //等待应答
heroistired 0:9b8df4f9b792 454 MPU_IIC_Send_Byte(data);//发送数据
heroistired 0:9b8df4f9b792 455 if(MPU_IIC_Wait_Ack()) //等待ACK
heroistired 0:9b8df4f9b792 456 {
heroistired 0:9b8df4f9b792 457 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 458 return 1;
heroistired 0:9b8df4f9b792 459 }
heroistired 0:9b8df4f9b792 460 MPU_IIC_Stop();
heroistired 0:9b8df4f9b792 461 return 0;
heroistired 0:9b8df4f9b792 462 }
heroistired 0:9b8df4f9b792 463 //IIC读一个字节
heroistired 0:9b8df4f9b792 464 //reg:寄存器地址
heroistired 0:9b8df4f9b792 465 //返回值:读到的数据
heroistired 0:9b8df4f9b792 466 u8 MPU_Read_Byte(u8 reg)
heroistired 0:9b8df4f9b792 467 {
heroistired 0:9b8df4f9b792 468 u8 res;
heroistired 0:9b8df4f9b792 469 MPU_IIC_Start();
heroistired 0:9b8df4f9b792 470 MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
heroistired 0:9b8df4f9b792 471 MPU_IIC_Wait_Ack(); //等待应答
heroistired 0:9b8df4f9b792 472 MPU_IIC_Send_Byte(reg); //写寄存器地址
heroistired 0:9b8df4f9b792 473 MPU_IIC_Wait_Ack(); //等待应答
heroistired 0:9b8df4f9b792 474 MPU_IIC_Start();
heroistired 0:9b8df4f9b792 475 MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令
heroistired 0:9b8df4f9b792 476 MPU_IIC_Wait_Ack(); //等待应答
heroistired 0:9b8df4f9b792 477 res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
heroistired 0:9b8df4f9b792 478 MPU_IIC_Stop(); //产生一个停止条件
heroistired 0:9b8df4f9b792 479 return res;
heroistired 0:9b8df4f9b792 480 }
heroistired 0:9b8df4f9b792 481
heroistired 0:9b8df4f9b792 482 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 483 //软件方法解算姿态
heroistired 0:9b8df4f9b792 484 ///////////////////////////////////////
heroistired 0:9b8df4f9b792 485
heroistired 0:9b8df4f9b792 486 /**************************************************************************
heroistired 0:9b8df4f9b792 487 函数功能：简易卡尔曼滤波
heroistired 0:9b8df4f9b792 488 入口参数：加速度、角速度
heroistired 0:9b8df4f9b792 489 返回 值：无
heroistired 0:9b8df4f9b792 490 **************************************************************************/
heroistired 0:9b8df4f9b792 491
heroistired 0:9b8df4f9b792 492 void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY)
heroistired 0:9b8df4f9b792 493 {
heroistired 0:9b8df4f9b792 494 X_Axis.angle+=(GyroX - X_Axis.Q_bias) * X_Axis.dt; //先验估计
heroistired 0:9b8df4f9b792 495 X_Axis.Pdot[0]=X_Axis.Q_angle - X_Axis.PP[0][1] - X_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分
heroistired 0:9b8df4f9b792 496
heroistired 0:9b8df4f9b792 497 X_Axis.Pdot[1]=-X_Axis.PP[1][1];
heroistired 0:9b8df4f9b792 498 X_Axis.Pdot[2]=-X_Axis.PP[1][1];
heroistired 0:9b8df4f9b792 499 X_Axis.Pdot[3]=X_Axis.Q_gyro;
heroistired 0:9b8df4f9b792 500 X_Axis.PP[0][0] += X_Axis.Pdot[0] * X_Axis.dt; // Pk-先验估计误差协方差微分的积分
heroistired 0:9b8df4f9b792 501 X_Axis.PP[0][1] += X_Axis.Pdot[1] * X_Axis.dt; // =先验估计误差协方差
heroistired 0:9b8df4f9b792 502 X_Axis.PP[1][0] += X_Axis.Pdot[2] * X_Axis.dt;
heroistired 0:9b8df4f9b792 503 X_Axis.PP[1][1] += X_Axis.Pdot[3] * X_Axis.dt;
heroistired 0:9b8df4f9b792 504
heroistired 0:9b8df4f9b792 505 X_Axis.Angle_err = AccelX - X_Axis.angle; //zk-先验估计
heroistired 0:9b8df4f9b792 506
heroistired 0:9b8df4f9b792 507 X_Axis.PCt_0 = X_Axis.C_0 * X_Axis.PP[0][0];
heroistired 0:9b8df4f9b792 508 X_Axis.PCt_1 = X_Axis.C_0 * X_Axis.PP[1][0];
heroistired 0:9b8df4f9b792 509
heroistired 0:9b8df4f9b792 510 X_Axis.E = X_Axis.R_angle + X_Axis.C_0 * X_Axis.PCt_0;
heroistired 0:9b8df4f9b792 511
heroistired 0:9b8df4f9b792 512 X_Axis.K_0 = X_Axis.PCt_0 / X_Axis.E;
heroistired 0:9b8df4f9b792 513 X_Axis.K_1 = X_Axis.PCt_1 / X_Axis.E;
heroistired 0:9b8df4f9b792 514
heroistired 0:9b8df4f9b792 515 X_Axis.t_0 = X_Axis.PCt_0;
heroistired 0:9b8df4f9b792 516 X_Axis.t_1 = X_Axis.C_0 * X_Axis.PP[0][1];
heroistired 0:9b8df4f9b792 517
heroistired 0:9b8df4f9b792 518 X_Axis.PP[0][0] -= X_Axis.K_0 * X_Axis.t_0; //后验估计误差协方差
heroistired 0:9b8df4f9b792 519 X_Axis.PP[0][1] -= X_Axis.K_0 * X_Axis.t_1;
heroistired 0:9b8df4f9b792 520 X_Axis.PP[1][0] -= X_Axis.K_1 * X_Axis.t_0;
heroistired 0:9b8df4f9b792 521 X_Axis.PP[1][1] -= X_Axis.K_1 * X_Axis.t_1;
heroistired 0:9b8df4f9b792 522
heroistired 0:9b8df4f9b792 523 X_Axis.angle += X_Axis.K_0 * X_Axis.Angle_err; //后验估计
heroistired 0:9b8df4f9b792 524 X_Axis.Q_bias += X_Axis.K_1 * X_Axis.Angle_err; //后验估计
heroistired 0:9b8df4f9b792 525 X_Axis.angle_dot = GyroX - X_Axis.Q_bias; //输出值(后验估计)的微分=角速度
heroistired 0:9b8df4f9b792 526
heroistired 0:9b8df4f9b792 527 Y_Axis.angle+=(GyroY - Y_Axis.Q_bias) * Y_Axis.dt; //先验估计
heroistired 0:9b8df4f9b792 528 Y_Axis.Pdot[0]=Y_Axis.Q_angle - Y_Axis.PP[0][1] - Y_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分
heroistired 0:9b8df4f9b792 529
heroistired 0:9b8df4f9b792 530 Y_Axis.Pdot[1]=-Y_Axis.PP[1][1];
heroistired 0:9b8df4f9b792 531 Y_Axis.Pdot[2]=-Y_Axis.PP[1][1];
heroistired 0:9b8df4f9b792 532 Y_Axis.Pdot[3]=Y_Axis.Q_gyro;
heroistired 0:9b8df4f9b792 533 Y_Axis.PP[0][0] += Y_Axis.Pdot[0] * Y_Axis.dt; // Pk-先验估计误差协方差微分的积分
heroistired 0:9b8df4f9b792 534 Y_Axis.PP[0][1] += Y_Axis.Pdot[1] * Y_Axis.dt; // =先验估计误差协方差
heroistired 0:9b8df4f9b792 535 Y_Axis.PP[1][0] += Y_Axis.Pdot[2] * Y_Axis.dt;
heroistired 0:9b8df4f9b792 536 Y_Axis.PP[1][1] += Y_Axis.Pdot[3] * Y_Axis.dt;
heroistired 0:9b8df4f9b792 537
heroistired 0:9b8df4f9b792 538 Y_Axis.Angle_err = AccelY - Y_Axis.angle; //zk-先验估计
heroistired 0:9b8df4f9b792 539
heroistired 0:9b8df4f9b792 540 Y_Axis.PCt_0 = Y_Axis.C_0 * Y_Axis.PP[0][0];
heroistired 0:9b8df4f9b792 541 Y_Axis.PCt_1 = Y_Axis.C_0 * Y_Axis.PP[1][0];
heroistired 0:9b8df4f9b792 542
heroistired 0:9b8df4f9b792 543 Y_Axis.E = Y_Axis.R_angle + Y_Axis.C_0 * Y_Axis.PCt_0;
heroistired 0:9b8df4f9b792 544
heroistired 0:9b8df4f9b792 545 Y_Axis.K_0 = Y_Axis.PCt_0 / Y_Axis.E;
heroistired 0:9b8df4f9b792 546 Y_Axis.K_1 = Y_Axis.PCt_1 / Y_Axis.E;
heroistired 0:9b8df4f9b792 547
heroistired 0:9b8df4f9b792 548 Y_Axis.t_0 = Y_Axis.PCt_0;
heroistired 0:9b8df4f9b792 549 Y_Axis.t_1 = Y_Axis.C_0 * Y_Axis.PP[0][1];
heroistired 0:9b8df4f9b792 550
heroistired 0:9b8df4f9b792 551 Y_Axis.PP[0][0] -= Y_Axis.K_0 * Y_Axis.t_0; //后验估计误差协方差
heroistired 0:9b8df4f9b792 552 Y_Axis.PP[0][1] -= Y_Axis.K_0 * Y_Axis.t_1;
heroistired 0:9b8df4f9b792 553 Y_Axis.PP[1][0] -= Y_Axis.K_1 * Y_Axis.t_0;
heroistired 0:9b8df4f9b792 554 Y_Axis.PP[1][1] -= Y_Axis.K_1 * Y_Axis.t_1;
heroistired 0:9b8df4f9b792 555
heroistired 0:9b8df4f9b792 556 Y_Axis.angle += Y_Axis.K_0 * Y_Axis.Angle_err; //后验估计
heroistired 0:9b8df4f9b792 557 Y_Axis.Q_bias += Y_Axis.K_1 * Y_Axis.Angle_err; //后验估计
heroistired 0:9b8df4f9b792 558 Y_Axis.angle_dot = GyroY - Y_Axis.Q_bias; //输出值(后验估计)的微分=角速度
heroistired 0:9b8df4f9b792 559 }
heroistired 0:9b8df4f9b792 560
heroistired 0:9b8df4f9b792 561 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 562 //功能 : 主中断的服务函数
heroistired 0:9b8df4f9b792 563 //参数 : 无
heroistired 0:9b8df4f9b792 564 //返回值 : 无
heroistired 0:9b8df4f9b792 565 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 566
heroistired 0:9b8df4f9b792 567 void MainInterruptIRQ(void)
heroistired 0:9b8df4f9b792 568 {
heroistired 0:9b8df4f9b792 569 float cpu_usage = 0;
heroistired 0:9b8df4f9b792 570 //系统监控部分代码
heroistired 0:9b8df4f9b792 571 TimeNow = SystemTimer.read_us();
heroistired 0:9b8df4f9b792 572 if(Flag_MeasureFreq)
heroistired 0:9b8df4f9b792 573 {
heroistired 0:9b8df4f9b792 574 FreqTime = TimeNow;
heroistired 0:9b8df4f9b792 575 FreqCounter = MainInterruptCounter;
heroistired 0:9b8df4f9b792 576 Flag_MeasureFreq = false;
heroistired 0:9b8df4f9b792 577 }
heroistired 0:9b8df4f9b792 578 if((MainInterruptCounter - FreqCounter) == 100)
heroistired 0:9b8df4f9b792 579 {
heroistired 0:9b8df4f9b792 580 MainInterruptFreq = 100.0 * 1000000 / (TimeNow - FreqTime);
heroistired 0:9b8df4f9b792 581 Flag_MeasureFreq = true;
heroistired 0:9b8df4f9b792 582 }
heroistired 0:9b8df4f9b792 583 MainInterruptCounter++;
heroistired 0:9b8df4f9b792 584
heroistired 0:9b8df4f9b792 585
heroistired 0:9b8df4f9b792 586 //temp=MPU_Get_Temperature(); //得到温度值
heroistired 0:9b8df4f9b792 587 MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
heroistired 0:9b8df4f9b792 588 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
heroistired 0:9b8df4f9b792 589 if(gyroy>32768) gyroy-=65536; //数据类型转换 也可通过short强制类型转换
heroistired 0:9b8df4f9b792 590 if(gyroz>32768) gyroz-=65536; //数据类型转换
heroistired 0:9b8df4f9b792 591 if(gyrox>32768) gyrox-=65536; //数据类型转换
heroistired 0:9b8df4f9b792 592 if(aacx>32768) aacx-=65536; //数据类型转换
heroistired 0:9b8df4f9b792 593 if(aacz>32768) aacz-=65536; //数据类型转换
heroistired 0:9b8df4f9b792 594 if(aacy>32768) aacy-=65536; //数据类型转换
heroistired 0:9b8df4f9b792 595
heroistired 0:9b8df4f9b792 596 ThetaXz=atan2((double)aacy,(double)aacz)*180/PI; //重力加速度在XOZ、YOZ面上与z轴夹角
heroistired 0:9b8df4f9b792 597 ThetaYz=atan2((double)aacx,(double)aacz)*180/PI; //计算倾角
heroistired 0:9b8df4f9b792 598 gyroy=gyroy/16.4; //陀螺仪量程转换
heroistired 0:9b8df4f9b792 599 gyrox=gyrox/16.4; //陀螺仪量程转换
heroistired 0:9b8df4f9b792 600 Kalman_Filter(ThetaXz,-gyrox, ThetaYz,-gyroy);
heroistired 0:9b8df4f9b792 601
heroistired 0:9b8df4f9b792 602 Processing(); //用户进程
heroistired 0:9b8df4f9b792 603 cpu_usage = (float)(SystemTimer.read_us() - TimeNow) / 10000; //计算CPU使用率
heroistired 0:9b8df4f9b792 604 if(cpu_usage > 0)
heroistired 0:9b8df4f9b792 605 CpuUsage = cpu_usage;
heroistired 0:9b8df4f9b792 606 }
heroistired 0:9b8df4f9b792 607
heroistired 0:9b8df4f9b792 608
heroistired 0:9b8df4f9b792 609
heroistired 0:9b8df4f9b792 610 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 611 //功能 : 系统初始化
heroistired 0:9b8df4f9b792 612 //参数 : 无
heroistired 0:9b8df4f9b792 613 //返回值 : 无
heroistired 0:9b8df4f9b792 614 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 615 void System_Init(void)
heroistired 0:9b8df4f9b792 616 {
heroistired 0:9b8df4f9b792 617 SystemTimer.start();
heroistired 0:9b8df4f9b792 618
heroistired 0:9b8df4f9b792 619 //TFT.DispInit();
heroistired 0:9b8df4f9b792 620 //TFT.FillScreen(ILI9340_BLACK);
heroistired 0:9b8df4f9b792 621 //TFT.SetRotation(3);
heroistired 0:9b8df4f9b792 622 //TFT.DrawString("System initing......", 50, 120, 3, ILI9340_WHITE);
heroistired 0:9b8df4f9b792 623
heroistired 0:9b8df4f9b792 624 MPU_Init();
heroistired 0:9b8df4f9b792 625 MainInterrupt.attach(&MainInterruptIRQ, 0.01);
heroistired 0:9b8df4f9b792 626
heroistired 0:9b8df4f9b792 627 Flag_MeasureFreq = true; //开始一次测量
heroistired 0:9b8df4f9b792 628
heroistired 0:9b8df4f9b792 629 X_Axis.K1 = 0.02;
heroistired 0:9b8df4f9b792 630 X_Axis.Q_angle=0.001;
heroistired 0:9b8df4f9b792 631 X_Axis.Q_gyro=0.003;
heroistired 0:9b8df4f9b792 632 X_Axis.R_angle=0.5;
heroistired 0:9b8df4f9b792 633 X_Axis.dt=0.005;
heroistired 0:9b8df4f9b792 634 X_Axis.C_0 = 1;
heroistired 0:9b8df4f9b792 635 X_Axis.Pdot[0] = 0;
heroistired 0:9b8df4f9b792 636 X_Axis.Pdot[1] = 0;
heroistired 0:9b8df4f9b792 637 X_Axis.Pdot[2] = 0;
heroistired 0:9b8df4f9b792 638 X_Axis.Pdot[3] = 0;
heroistired 0:9b8df4f9b792 639 X_Axis.PP[0][0]=1;
heroistired 0:9b8df4f9b792 640 X_Axis.PP[0][1]=0;
heroistired 0:9b8df4f9b792 641 X_Axis.PP[1][0]=1;
heroistired 0:9b8df4f9b792 642 X_Axis.PP[1][1]=0;
heroistired 0:9b8df4f9b792 643
heroistired 0:9b8df4f9b792 644 Y_Axis.K1 = 0.02;
heroistired 0:9b8df4f9b792 645 Y_Axis.Q_angle=0.001;
heroistired 0:9b8df4f9b792 646 Y_Axis.Q_gyro=0.003;
heroistired 0:9b8df4f9b792 647 Y_Axis.R_angle=0.5;
heroistired 0:9b8df4f9b792 648 Y_Axis.dt=0.005;
heroistired 0:9b8df4f9b792 649 Y_Axis.C_0 = 1;
heroistired 0:9b8df4f9b792 650 Y_Axis.Pdot[0] = 0;
heroistired 0:9b8df4f9b792 651 Y_Axis.Pdot[1] = 0;
heroistired 0:9b8df4f9b792 652 Y_Axis.Pdot[2] = 0;
heroistired 0:9b8df4f9b792 653 Y_Axis.Pdot[3] = 0;
heroistired 0:9b8df4f9b792 654 Y_Axis.PP[0][0]=1;
heroistired 0:9b8df4f9b792 655 Y_Axis.PP[0][1]=0;
heroistired 0:9b8df4f9b792 656 Y_Axis.PP[1][0]=1;
heroistired 0:9b8df4f9b792 657 Y_Axis.PP[1][1]=0;
heroistired 0:9b8df4f9b792 658 }
heroistired 0:9b8df4f9b792 659
heroistired 0:9b8df4f9b792 660 //此处--推荐--配置外设、定义全局变量
heroistired 0:9b8df4f9b792 661 //-------------------代码块开始
heroistired 0:9b8df4f9b792 662
heroistired 0:9b8df4f9b792 663 //DigitalOut myled(LED1);
heroistired 0:9b8df4f9b792 664 AnalogIn LeftJoystickX(Pin_LeftJoystickX);
heroistired 0:9b8df4f9b792 665 AnalogIn LeftJoystickY(Pin_LeftJoystickY);
heroistired 0:9b8df4f9b792 666 AnalogIn RightJoystickX(Pin_RightJoystickX);
heroistired 0:9b8df4f9b792 667 AnalogIn RightJoystickY(Pin_RightJoystickY);
heroistired 0:9b8df4f9b792 668
heroistired 0:9b8df4f9b792 669 float LeftJoystickValueX = 0.0, LeftJoystickValueY = 0.0, RightJoystickValueX = 0.0, RightJoystickValueY = 0.0; //摇杆x,y轴的读数 归一化到 0-1
heroistired 0:9b8df4f9b792 670
heroistired 0:9b8df4f9b792 671 Servo Servo1(Pin_Servo1);
heroistired 0:9b8df4f9b792 672 Servo Servo2(Pin_Servo2);
heroistired 0:9b8df4f9b792 673 Servo Servo3(Pin_Servo3);
heroistired 0:9b8df4f9b792 674 Servo Servo4(Pin_Servo4);
heroistired 0:9b8df4f9b792 675 Servo Servo5(Pin_Servo5);
heroistired 0:9b8df4f9b792 676 Servo Servo6(Pin_Servo6);
heroistired 0:9b8df4f9b792 677 Servo Servo7(Pin_Servo7);
heroistired 0:9b8df4f9b792 678 Servo Servo8(Pin_Servo8);
heroistired 0:9b8df4f9b792 679 Servo Servo9(Pin_Servo9);
heroistired 0:9b8df4f9b792 680 Servo Servo10(Pin_Servo10);
heroistired 0:9b8df4f9b792 681 Servo Servo11(Pin_Servo11);
heroistired 0:9b8df4f9b792 682 Servo Servo12(Pin_Servo12);
heroistired 0:9b8df4f9b792 683
heroistired 0:9b8df4f9b792 684 Serial UserUart(PC_10, PC_11);
heroistired 0:9b8df4f9b792 685
heroistired 0:9b8df4f9b792 686
heroistired 0:9b8df4f9b792 687 //-------------------代码块结束
heroistired 0:9b8df4f9b792 688
heroistired 0:9b8df4f9b792 689 int i = 0, j;
heroistired 0:9b8df4f9b792 690 float degree1 = 45.0, degree2 = 135.0;
heroistired 0:9b8df4f9b792 691 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 692 //功能 : 用户进程函数 用于执行用户自定义的后台任务 执行频率是100Hz
heroistired 0:9b8df4f9b792 693 //参数 : 无
heroistired 0:9b8df4f9b792 694 //返回值 : 无
heroistired 0:9b8df4f9b792 695 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:9b8df4f9b792 696 void Processing(void)
heroistired 0:9b8df4f9b792 697 {
heroistired 0:9b8df4f9b792 698 LeftJoystickValueX = LeftJoystickX.read();
heroistired 0:9b8df4f9b792 699 LeftJoystickValueY = LeftJoystickY.read();
heroistired 0:9b8df4f9b792 700 RightJoystickValueX = RightJoystickX.read();
heroistired 0:9b8df4f9b792 701 RightJoystickValueY = RightJoystickY.read();
heroistired 0:9b8df4f9b792 702 if((MainInterruptCounter % 50) == 0)
heroistired 0:9b8df4f9b792 703 {
heroistired 0:9b8df4f9b792 704 if(i == 0)
heroistired 0:9b8df4f9b792 705 {
heroistired 0:9b8df4f9b792 706 i = !i;
heroistired 0:9b8df4f9b792 707 Servo1.SetDegree(degree1);
heroistired 0:9b8df4f9b792 708 Servo2.SetDegree(degree1);
heroistired 0:9b8df4f9b792 709 Servo3.SetDegree(degree1);
heroistired 0:9b8df4f9b792 710 Servo4.SetDegree(degree1);
heroistired 0:9b8df4f9b792 711 Servo5.SetDegree(degree1);
heroistired 0:9b8df4f9b792 712 Servo6.SetDegree(degree1);
heroistired 0:9b8df4f9b792 713 Servo7.SetDegree(degree1);
heroistired 0:9b8df4f9b792 714 Servo8.SetDegree(degree1);
heroistired 0:9b8df4f9b792 715 Servo9.SetDegree(degree1);
heroistired 0:9b8df4f9b792 716 Servo10.SetDegree(degree1);
heroistired 0:9b8df4f9b792 717 Servo11.SetDegree(degree1);
heroistired 0:9b8df4f9b792 718 Servo12.SetDegree(degree1);
heroistired 0:9b8df4f9b792 719 }
heroistired 0:9b8df4f9b792 720 else
heroistired 0:9b8df4f9b792 721 {
heroistired 0:9b8df4f9b792 722 i = !i;
heroistired 0:9b8df4f9b792 723 Servo1.SetDegree(degree2);
heroistired 0:9b8df4f9b792 724 Servo2.SetDegree(degree2);
heroistired 0:9b8df4f9b792 725 Servo3.SetDegree(degree2);
heroistired 0:9b8df4f9b792 726 Servo4.SetDegree(degree2);
heroistired 0:9b8df4f9b792 727 Servo5.SetDegree(degree2);
heroistired 0:9b8df4f9b792 728 Servo6.SetDegree(degree2);
heroistired 0:9b8df4f9b792 729 Servo7.SetDegree(degree2);
heroistired 0:9b8df4f9b792 730 Servo8.SetDegree(degree2);
heroistired 0:9b8df4f9b792 731 Servo9.SetDegree(degree2);
heroistired 0:9b8df4f9b792 732 Servo10.SetDegree(degree2);
heroistired 0:9b8df4f9b792 733 Servo11.SetDegree(degree2);
heroistired 0:9b8df4f9b792 734 Servo12.SetDegree(degree2);
heroistired 0:9b8df4f9b792 735 }
heroistired 0:9b8df4f9b792 736 }
heroistired 0:9b8df4f9b792 737 }
heroistired 0:9b8df4f9b792 738
heroistired 0:9b8df4f9b792 739 int main()
heroistired 0:9b8df4f9b792 740 {
heroistired 0:9b8df4f9b792 741 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类的定义之后否则无法正常工作
heroistired 0:9b8df4f9b792 742 TFT_LED = 1; //点亮TFT背光
heroistired 0:9b8df4f9b792 743 TFT.DispInit();
heroistired 0:9b8df4f9b792 744 TFT.FillScreen(ILI9340_BLACK);
heroistired 0:9b8df4f9b792 745 TFT.SetRotation(3);
heroistired 0:9b8df4f9b792 746 TFT.DrawString("iCenter", 76, 86, 3, ILI9340_WHITE);
heroistired 0:9b8df4f9b792 747 TFT.DrawString("System initing...", 24, 146, 2, ILI9340_WHITE);
heroistired 0:9b8df4f9b792 748 System_Init();
heroistired 0:9b8df4f9b792 749 TFT.DrawString("iCenter", 76, 86, 3, ILI9340_BLACK);
heroistired 0:9b8df4f9b792 750 TFT.DrawString("System initing...", 24, 146, 2, ILI9340_BLACK);
heroistired 0:9b8df4f9b792 751
heroistired 0:9b8df4f9b792 752 while(true) {
heroistired 0:9b8df4f9b792 753 printf("CPU Usage : %.2f\r\n", CpuUsage);
heroistired 0:9b8df4f9b792 754 printf("Pitch : %.2f\r\n", Y_Axis.angle);
heroistired 0:9b8df4f9b792 755 printf("Roll : %.2f\r\n", X_Axis.angle);
heroistired 0:9b8df4f9b792 756 printf("LeftJoystickValueX : %.2f\r\n", LeftJoystickValueX);
heroistired 0:9b8df4f9b792 757 printf("LeftJoystickValueY : %.2f\r\n", LeftJoystickValueY);
heroistired 0:9b8df4f9b792 758 printf("RightJoystickValueX : %.2f\r\n", RightJoystickValueX);
heroistired 0:9b8df4f9b792 759 printf("RightJoystickValueY : %.2f\r\n", RightJoystickValueY);
heroistired 0:9b8df4f9b792 760
heroistired 0:9b8df4f9b792 761 // Small amount of text to the display.
heroistired 0:9b8df4f9b792 762
heroistired 0:9b8df4f9b792 763
heroistired 0:9b8df4f9b792 764 // Do the waiting thang...
heroistired 0:9b8df4f9b792 765 wait(0.5);
heroistired 0:9b8df4f9b792 766
heroistired 0:9b8df4f9b792 767 }
heroistired 0:9b8df4f9b792 768 }