自己寫的stm32小四軸程序用mpu6050 dmp直接輸出姿態角裸機代碼

ID:177825 · 發表于 2017-3-21 20:22

自己寫的小四軸程序stm32f103c8t6，采用mpu6050中的dmp直接輸出姿態角可不用了解四元數，通行模塊為藍牙模塊，程序是裸機代碼沒有太多變量可以容易看懂，如果有這方面的愛好可以一起探討qq1244450581，PID算法只用了串級PD（這個PID是由模電穩壓的過程得到靈感）不過有引進PID死區，分為倆個PID調節

自己試了感覺挺穩，由于不能發太大文件遙控程序就不發了。

部分源程序預覽：

/*******************************************************************************
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);
//
}
…………余下代碼請下載附件…………