自己写的stm32小四轴程序用mpu6050 dmp直接输出姿态角裸机代码 -

/*******************************************************************************
gggi
四轴飞控程序
由mpu_dmp_get_data(&pitch,&roll,&yaw)这个函数引出欧拉角
由RCDataProcess(&exthrottle,&exroll,&expitch,&exyaw);这个引出期望角与油门
*******************************************************************************/
#include "public.h"
#include "printf.h"
#include "stdio.h"
#include "delay.h"
#include "dmp.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "bt.h"
#include "pwm.h"
#include "time.h" //把LED初始化的文件一块放里面，恩，我没有偷懒！！！
#include "pid.h"
float old;//用于判断是否进入加速剂调节模式
float exthrottle,roll_ex,pitch_ex,yaw_ex;
u16 power1,power2,power3,power4;
//S_FLOAT_ANGLE Q_ANGLE;
float pitch,roll,yaw; //欧拉角
float gyrox,gyroy,gyroz;
float Gyrox,Gyroy,Gyroz;
float aacx,aacy,aacz; //加速度传感器原始数据
//传送数据给匿名四轴上位机软件(V2.6版本)
//fun:功能字. 0XA0~0XAF
//data:数据缓存区,最多28字节!!
//len:data区有效数据个数
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
u8 send_buf[32];
u8 i;
if(len>28)return; //最多28字节数据
send_buf[len+3]=0; //校验数置零
send_buf[0]=0X88; //帧头
send_buf[1]=fun; //功能字
send_buf[2]=len; //数据长度
for(i=0;i<len;i++)send_buf[3+i]=data[i]; //复制数据
for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //计算校验和
for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //发送数据到串口1
}
//匿名四轴上位机发送的数据格式
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
{
u8 tbuf[28];
u8 i;
for(i=0;i<28;i++)tbuf[i]=0;//清0
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
tbuf[18]=(roll>>8)&0XFF;
tbuf[19]=roll&0XFF;
tbuf[20]=(pitch>>8)&0XFF;
tbuf[21]=pitch&0XFF;
tbuf[22]=(yaw>>8)&0XFF;
tbuf[23]=yaw&0XFF;
usart1_niming_report(0XAF,tbuf,28);//飞控显示帧,0XAF
}
int main()
{
u8 i;
float j;//用于遥控油门数值
u8 a,b=0,x=0;//用于加速计的累加求平均值
u16 flag=0,gyro[3];
float aac1[10],aac2[10];
SystemInit();
delay_init(72);
LED_Init() ;
printf_init();
I2C_GPIO_Config();
delay_ms(10);
PIDvalue();//往PID里面塞东西，确定三个姿态的p，i，d值
MPU_Init();
mpu_dmp_init();
pwm_init();
TIM2_Init();
while(1)
{
mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
////////////////////////////////////加速剂平滑滤波
// aac1[x]=aacx;
//aac2[x]=aacy;
// x++;
// for(i=0;i<10;i++)
// aacx=(aacx+aac1[i])/2;
//
// for(i=0;i<10;i++)
// aacy=(aacy+aac2[i])/2;
// if(x==10)
// x=0;
/////////////////////////////////////
if(timer==1)
{
timer=0;
flag++;
gyrox=gyrox/10;
gyroy=gyroy/10;
gyroz=gyroz/10;
// aacx=aacx/40-25;
// aacy=aacy/40+22.5;
// aacz=aacz/90-180;
// RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
/////////////////////////翻滚程序/////////////////////
/////////////////////////////////////////////////////////////////////////
//////////////////////////////////油门保护程序////////////////////////////////
// if((old-exthrottle)>400)
// {
// while(1)
// {
// mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz);
// RCDataProcess(&nouse,&roll_ex,&pitch_ex,&yaw_ex);
// exthrottle=old-10;
// singlePIDupdate();
// pwm_in(power1,power2,power3,power4);
// if(nouse>300)break;
// }
// }
//////////////////////////////////////////////////////////////////////////////////////
RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
// if(roll_ex==170)
// {
// pwm_in(power1+200,power2-200,power3-200,power4+200);
// delay_ms(10);
// pwm_in(power1-200,power2+400,power3+400,power4-200);
// delay_ms(10);
//
// power2=power2+400;
// if(power2>1000)
// power2=1000;
// power4=power4-200;
// if(power4<0)
// power4=0;
//
// power3=power3+400;
// if(power3>1000)
// power3=1000;
// power1=power1-200;
// if(power1<0)
// power1=0;
// while(1)
// {
// pwm_in(power1,power2,power3,power4);
// mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
// if(roll_ex>150)
// {
// b=1;
// delay_ms(500);
// }
// if((roll>(-40))&&(b==1))
// {b=0;
// break;
// }
// }
// RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
// mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
// gyrox=gyrox/10;
// gyroy=gyroy/10;
// gyroz=gyroz/10;
// aacx=aacx/40-25;
// aacy=aacy/40+22.5;
// aacz=aacz/90-180;
// }
//exthrottle=21;
if(exthrottle>20)
{
// Q_ANGLE.Pitch=(int)((pitch)*100)/100.0;
// Q_ANGLE.Roll=(int)(roll*100)/100.0;
// Q_ANGLE.Yaw=(int)(yaw*100)/100.0;
singlePIDupdate(); //PID调节
pwm_in(power1,power2,power3,power4); //油门输出
//pwm_in(power1,0,power3,0); //油门输出
// pwm_in(0,power2,0,power4);
}
else if(exthrottle<20)
{
// Q_ANGLE.Pitch=(int)((pitch)*100)/100.0+1.5;
// Q_ANGLE.Roll=(int)(roll*100)/100.0+0.3;
// Q_ANGLE.Yaw=(int)(Q_ANGLE.Yaw*100)/100.0;
pwm_in(0,0,0,0); //油门输出
}
if(flag==500)
{
led_display1();
//MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
//usart1_report_imu(exthrottle,roll_ex,pitch_ex,gyrox,gyrox,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
// j=exthrottle+1000;
// i[0]=((int)j>>8)&0XFF;
// i[1]=(int)j&0XFF;
//
// usart1_niming_report(0xAE,i,24);
}
if(flag==1000)
{
flag=0;
led_display2();
old=exthrottle;
}
if(pitch>75||pitch<-75||roll<-75||roll>75)
{
pwm_in(0,0,0,0);
while(1);
}
//}
// if(flag==30)
// {
// //led_display1();
//
// flag=0;
// usart1_report_imu(exthrottle,roll_ex,pitch_ex,gyrox,gyrox,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
//// j=exthrottle+1000;
//// i[0]=((int)j>>8)&0XFF;
//// i[1]=(int)j&0XFF;
////
//// usart1_niming_report(0xAE,i,24);
//
}
…………余下代码请下载附件…………

复制代码