自制四轴飞行器飞行控制器

wsnfly

下一步打算做机架和电调，不过先学STM32F4

话不多说，直接上视频，从头到尾电路板制作过程和调试过程，到最后的加上图传飞行。

主控程序如下

#include "aquanjubianliang.c"

#include "usart.h"

#include "delay.h"

#include "led.h" 

#include "key.h"

#include "exti.h"

#include "wdg.h"

#include "timer.h"

#include "lcd.h"    

#include "rtc.h"

#include "wkup.h"

#include "adc.h"

#include "dma.h"

#include "24cxx.h"

#include "flash.h"

#include "24l01.h"

#include "mmc_sd.h"

#include "remote.h"

#include "ds18b20.h"

#include "mouse.h"

#include "text.h"

#include "fat.h"

#include "fontupd.h"

#include "IIC.h"

#include "inv_mpu.h"

#include "inv_mpu_dmp_motion_driver.h"

#include "math.h"

zhinengchuankou();

#define duoji1 PAout(5)// PA8

#define duoji2 PAout(1)

#define duoji3 PAout(0)

#define duoji4 PAout(2)

s16 duojipianyi[4];

//u16 jiaosuduchangshu=50,jieshoulingmin=4,shijijiaosuduchangshu=50,zuidaxiuzhengsudu=60,zuixiaotiaozhengshudu=10,weitiaoweizhi=10,jiaosuduqudiancishu=5,zuidatiaozhengsudu=40;//可用

//jiaosuduchangshu计算出各轴需要的理论角速度用；jieshoulingmin由于接收机定时器有误差，消除误差用，会消除精度； shijijiaosuduchangshu用于取得角速度之后除数；zuidaxiuzhengsudu自稳系统的最大旋转速度

//zuixiaotiaozhengshudu当调整速度小于此数值时，增加调整速度,weitiaoweizhi当调整速度小于此数值时 进行微调就可以,jiaosuduqudiancishu角速度的次数累加，越多越准确，但反应越慢,zuidatiaozhengsudu表示最大稳定时的速度

u16 jiaosuduchangshu=100,jieshoulingmin=4,shijijiaosuduchangshu2=50,zuidaxiuzhengsudu=250,zuixiaotiaozhengshudu=6,weitiaoweizhi=2,jiaosuduqudiancishu=2,zuidatiaozhengsudu=60;

//kongzhilingmin接收机的灵敏度用

u32 kongzhilingmin=30000;

u16 jieshouji[6],wei=0,jiesuo=0,jiaosuducishu=0,shijijiaosuduchangshu=20;

s32 h,duojikongzhi[8],i,zhongduancishu,xlilunjiaosudu,ylilunjiaosudu,zlilunjiaosudu,xpianyijiaosudu,ypianyijiaosudu,zpianyijiaosudu,xjiaosudu,yjiaosudu,zjiaosudu,lingshijiaosudu,xjsd[100],yjsd[100],zjsd[100];

double shurufuyi,shurushengjiang,shurufangxiang,shuruyoumen,t,pi=3.14159,youmen[10];

double nifuyi=0,nishengjiang=0,nifangxiang=0,niyoumen=0;

double lilunfuyi,lilunshengjiang,lilunfangxiang,lilunyoumen,chafuyi,chashengjiang,chafangxiang,chayoumen,shijiyoumen;

u8 shuruyinjiao=10,suo=0,SendBuff[256]={97};

int main(void)

{

//    u16 i;

{

duojipianyi[0]=0;

duojipianyi[1]=0;

duojipianyi[2]=0;

duojipianyi[3]=0;

  duojikongzhi[0]=1100;

duojikongzhi[1]=1100;

duojikongzhi[2]=1100;

duojikongzhi[3]=1100;

duojikongzhi[4]=2000;

}

   Stm32_Clock_Init(9); //系统时钟设置

delay_init(72); //延时初始化

uart_init(72,1600);  //串口1初始化  

//USART1->DR=SendBuff[1];

gpioshezhi(); //引脚设置

led4=0;

// LED_Init();          //LED初始化

i2cInit(); //I2C接口设置

//Timer4_Init(7199,1000);

Timer3_Init(10000,72); //定时器3，中断开启，时间不定，输出PWM信号控制螺旋桨转速   抢断优先级

Timer2_Init(7199,70); //定时器2，有中断，70毫秒读取陀螺仪数据

Timer1_Init(60000,72);TIM1->CR1|=0x01;//定时器一，没有中断，用于计时接收机信号

EXTI15_10_Init(); //10-15中断 接收机信号中断 抢断优先级

delay_ms(100); //延时300MS

chuankoufasong(36,1); //串口发送，检测芯片是否运行

result = mpu_init(); //dmp读出

delay_ms(100); //延时300MS

{//去角速度偏移量，MPU6050制造误差

xpianyijiaosudu=dumpu6050jicunqi(0);

ypianyijiaosudu=dumpu6050jicunqi(1);

zpianyijiaosudu=dumpu6050jicunqi(2);

}

if(!result)

  {    

chuankoufasong(36,2);

mpufifo_init(); //dmp——fifo设置

  }

MYDMA_Enable(DMA1_Channel4);

chuankoufasong(37,4);

USART1->DR=SendBuff[chuankoufasongwei];

   while(1) 

{

{ //计算出理论飞行角度

/////************************************************************************//

lilunyoumen=shuruyoumen;

lilunfuyi=0.4*(shurufuyi/300);

lilunshengjiang=0.4*(shurushengjiang/300);

lilunfangxiang=lilunfangxiang+nifangxiang*2 ;

nifuyi=0;

nishengjiang=0;

nifangxiang=0;

if(lilunshengjiang>3.14159/2){lilunfuyi=lilunfuyi+3.14159;lilunfangxiang=lilunfangxiang-3.14159;lilunshengjiang=3.1415/2;}

if(lilunshengjiang<-3.14159/2){lilunfuyi=lilunfuyi+3.14159;lilunfangxiang=lilunfangxiang-3.14159;lilunshengjiang=-3.1415/2;}

if(lilunfangxiang>2*3.14159)lilunfangxiang=lilunfangxiang-2*3.14159;

if(lilunfangxiang<0)lilunfangxiang=lilunfangxiang+3.14159*2;

if(lilunfuyi>3.14159)lilunfuyi=lilunfuyi-3.14159*2;

if(lilunfuyi<-3.14159)lilunfuyi=lilunfuyi+3.14159*2;

}

{ //计算出实际坐标和理论坐标差

chafuyi=Roll-lilunfuyi;

if(chafuyi>pi)chafuyi-=2*pi;

if(chafuyi<-pi)chafuyi+=2*pi;

//if(chafuyi>(pi/8))chafuyi=pi/8;

//if(chafuyi<(-pi)/8)chafuyi=-pi/8;

chashengjiang=-(Pitch-lilunshengjiang);

if(chashengjiang>(pi))chashengjiang-=2*pi;

if(chashengjiang<-(pi))chashengjiang+=2*pi;

//if(chashengjiang>(pi/8))chashengjiang=pi/8;

//if(chashengjiang<(-pi)/8)chashengjiang=-pi/8; 

chafangxiang=-(Yaw-lilunfangxiang);

if(chafangxiang>pi)chafangxiang-=2*pi;

if(chafangxiang<-pi)chafangxiang+=2*pi;

//if(chafangxiang>(pi/8))chafangxiang=pi/8;

//if(chafangxiang<(-pi)/8)chafangxiang=-pi/8;

}

{ //计算出飞行器各个轴需要的理论角速度 

xlilunjiaosudu=(chafuyi )*jiaosuduchangshu;

ylilunjiaosudu=(chashengjiang*cos(Roll)+chafangxiang*sin(Roll))*jiaosuduchangshu;

zlilunjiaosudu=(chafangxiang*cos(Roll)-chashengjiang*sin(Roll))*jiaosuduchangshu;

if(xlilunjiaosudu>zuidatiaozhengsudu)xlilunjiaosudu=zuidatiaozhengsudu;

if(xlilunjiaosudu<-zuidatiaozhengsudu)xlilunjiaosudu=-zuidatiaozhengsudu;

if(ylilunjiaosudu>zuidatiaozhengsudu)ylilunjiaosudu=zuidatiaozhengsudu;

if(ylilunjiaosudu<-zuidatiaozhengsudu)ylilunjiaosudu=-zuidatiaozhengsudu;

if(zlilunjiaosudu>zuidatiaozhengsudu)zlilunjiaosudu=zuidatiaozhengsudu;

if(zlilunjiaosudu<-zuidatiaozhengsudu)zlilunjiaosudu=-zuidatiaozhengsudu;

if(xlilunjiaosudu<zuixiaotiaozhengshudu&&xlilunjiaosudu>weitiaoweizhi)xlilunjiaosudu=zuixiaotiaozhengshudu;

if(ylilunjiaosudu<zuixiaotiaozhengshudu&&ylilunjiaosudu>weitiaoweizhi)ylilunjiaosudu=zuixiaotiaozhengshudu;

if(zlilunjiaosudu<zuixiaotiaozhengshudu&&zlilunjiaosudu>weitiaoweizhi)zlilunjiaosudu=zuixiaotiaozhengshudu;

if(xlilunjiaosudu>-zuixiaotiaozhengshudu&&xlilunjiaosudu<-weitiaoweizhi)xlilunjiaosudu=-zuixiaotiaozhengshudu;

if(ylilunjiaosudu>-zuixiaotiaozhengshudu&&ylilunjiaosudu<-weitiaoweizhi)ylilunjiaosudu=-zuixiaotiaozhengshudu;

if(zlilunjiaosudu>-zuixiaotiaozhengshudu&&zlilunjiaosudu<-weitiaoweizhi)zlilunjiaosudu=-zuixiaotiaozhengshudu;

}

{   //计算角速度平均数

xjiaosudu=0;

yjiaosudu=0;

zjiaosudu=0;

for(i=0;i<jiaosuduqudiancishu;i++)

{

xjiaosudu+=xjsd;

yjiaosudu+=yjsd;

zjiaosudu+=zjsd;

}

xjiaosudu/=jiaosuduqudiancishu;

yjiaosudu/=-jiaosuduqudiancishu;

zjiaosudu=zjiaosudu/jiaosuduqudiancishu;

}

if(shuruyoumen>1200) //如果油门处于激活状态

{ //从理论角速度计算功电机运行的信号强度

shijijiaosuduchangshu=shijijiaosuduchangshu2*(lilunyoumen-800)/1000*(lilunyoumen-600)/1000;

lingshijiaosudu=(-xjiaosudu)/shijijiaosuduchangshu;

if(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;

if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;

//xjiaosudu=-shurufuyi/jiaosuduchangshu-lingshijiaosudu;//角速度调整模式时 jiaosuduchangshu=15

//xlilunjiaosudu=-shurufuyi/jiaosuduchangshu;

xjiaosudu=xlilunjiaosudu-lingshijiaosudu;

lingshijiaosudu=(yjiaosudu)/shijijiaosuduchangshu;

if(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;

if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;

//yjiaosudu=shurushengjiang/jiaosuduchangshu-lingshijiaosudu;

//ylilunjiaosudu=shurushengjiang/jiaosuduchangshu;

yjiaosudu=ylilunjiaosudu-lingshijiaosudu;

lingshijiaosudu=(zjiaosudu)/shijijiaosuduchangshu;

if(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;

if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;

//zjiaosudu=shurufangxiang/jiaosuduchangshu-lingshijiaosudu;

//zlilunjiaosudu=shurufangxiang/jiaosuduchangshu;

zjiaosudu=zlilunjiaosudu-lingshijiaosudu;

if(lilunyoumen>1700)lilunyoumen=1700;

duojikongzhi[0]=lilunyoumen+xjiaosudu+yjiaosudu+zjiaosudu*3+duojipianyi[0];

duojikongzhi[1]=lilunyoumen-xjiaosudu+yjiaosudu-zjiaosudu*3+duojipianyi[1];

duojikongzhi[2]=lilunyoumen-xjiaosudu-yjiaosudu+zjiaosudu*3+duojipianyi[2];

duojikongzhi[3]=lilunyoumen+xjiaosudu-yjiaosudu-zjiaosudu*3+duojipianyi[3];

for(i=0;i<4;i++)

{

if(duojikongzhi<1200)duojikongzhi=1200;

}

}

else { //如果油门为0

duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=1100;

lilunfuyi=Roll;

lilunshengjiang=Pitch;

lilunfangxiang=Yaw;

}

//t=double(jieshouji[0])/100;

/*

for(i=0;i<6;i++){

}

chuankoufasong(36,shurufuyi);

chuankoufasong(37,shurushengjiang);

chuankoufasong(38,shuruyoumen);

chuankoufasong(39,shurufangxiang);delay_ms(30);

{ //发送串口数据，功电脑读取.

h=shurufangxiang;

chuankoufasong(36,xjiaosudu); delay_ms(10);

h=shuruyoumen;

chuankoufasong(37,yjiaosudu); delay_ms(10);

h=chafangxiang*57;

chuankoufasong(38,zjiaosudu);

delay_ms(10);

chuankoufasong(39,duojikongzhi[3]);

delay_ms(10);

//duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=shuruyoumen+1100;

}

h=xjiaosudu;

chuankoufasong(36,h);

h=yjiaosudu;

chuankoufasong(37,h); delay_ms(10);

h=zjiaosudu;

chuankoufasong(38,h);

h=shurufangxiang;

chuankoufasong(39,h);

h=shuruyoumen;

chuankoufasong(39,jieshouji[4]);

h= dumpu6050jicunqi(0);

chuankoufasong(37,h);

h= dumpu6050jicunqi(1);

chuankoufasong(38,h);

h= dumpu6050jicunqi(2);

chuankoufasong(39,h);

chuankoufasong(36,jieshouji[3]);

chuankoufasong(37,jieshouji[2]);

chuankoufasong(38,jieshouji[4]);

*/

{

if((USART1->SR&0X40)!=0)

{

zhinengchuankou();

}

}

}

}

void TIM2_IRQHandler(void) //定时器2 ，用于读取DMP数据

{ 

TIM2->SR&=~(1<<0);//清除中断标志位                  

  

 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);  

     if (sensors & INV_WXYZ_QUAT )

 {

   q0=quat[0] / q30;

 q1=quat[1] / q30;

 q2=quat[2] / q30;

 q3=quat[3] / q30;

 Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); // roll副翼

 itch  = asin(-2 * q1 * q3 + 2 * q0* q2); // pitch升降

   Yaw =  atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) ;//方向

 //h=Roll*57295;

 //chuankoufasong(36,h);

 //h=Pitch*57295;

 //chuankoufasong(37,h);

 //h=Yaw*57295;

// chuankoufasong(38,h);

// PrintChar("nihao\n");

}

xjsd[jiaosuducishu]=(gyro[0]-0);

yjsd[jiaosuducishu]=-(gyro[1]-0);

zjsd[jiaosuducishu]=(gyro[2]-0);

jiaosuducishu++;

if(jiaosuducishu>=jiaosuduqudiancishu)jiaosuducishu=0;

}

void EXTI15_10_IRQHandler(void) //接收机信号接收中断         耗时2微秒  以内

{

u8 jicixunhuan;

for(jicixunhuan=12;jicixunhuan<16;jicixunhuan++)

{

if(((GPIOB->IDR>>jicixunhuan)&1)){

shuruyinjiao=jicixunhuan;

TIM1->CNT=0;

}

}

if(!(GPIOB->IDR>>12&0xf)){ //如果接收机发送结束

if(TIM1->CNT>1000&&TIM1->CNT<2000)

{

jieshouji[shuruyinjiao-10]=TIM1->CNT; //将接收机变量保存到变量

jieshouji[shuruyinjiao-10]/=jieshoulingmin;

jieshouji[shuruyinjiao-10]*=jieshoulingmin;

}

if(shuruyinjiao==15)

{

/////************************************************************************//

//计算得到每个摇杆的行程

if((youmen[0]-jieshouji[3])<100&&(shuruyoumen-jieshouji[3])<100)

{

shurufuyi=1492-jieshouji[3];

}

youmen[0]=jieshouji[3];

shurushengjiang=1492-jieshouji[2];

shurufangxiang=1492-jieshouji[5];

shuruyoumen=jieshouji[4];

/////************************************************************************//

//每个遥感的行程转化成每次的偏移量

nifuyi=nifuyi+shurufuyi/kongzhilingmin;

nishengjiang=nishengjiang+shurushengjiang/kongzhilingmin;

nifangxiang=nifangxiang+shurufangxiang/kongzhilingmin;

}

}

if(shuruyoumen<1190){jiesuo++;

if(shurufangxiang>200){

if(jiesuo>700) {

jiesuo=0;

suo=0;

}

}

if(shurufangxiang<-200){

if(jiesuo>700){

jiesuo=0;

suo=1;

}

}

}

else jiesuo=0;

zhongduancishu++;

EXTI->R=1<<10;

EXTI->R=1<<11;

EXTI->R=1<<12;

EXTI->R=1<<13;

EXTI->R=1<<14;

EXTI->R=1<<15;

}

void TIM3_IRQHandler(void) //定时器3，用于输出PWM信号   耗时1微秒 以内

{ 

if(suo==1){

if(shuruyoumen<1200)duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=1100;

TIM3->CNT=duojikongzhi[wei];

if(wei==0){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji1=1;}

if(wei==1){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji2=1;}

if(wei==2){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji3=1;}

if(wei==3){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji4=1;led4=!led4;}

if(wei==4){duoji1=0;duoji2=0;duoji3=0;duoji4=0; }

                

wei++;

if(wei>4)wei=0;

}

if(suo==0)led4=0;

TIM3->SR&=~(1<<0);//清除中断标志位  

}

void TIM4_IRQHandler(void)

{

TIM4->SR&=~(1<<0);//清除中断标志?

/*? 

*/

}

zhinengchuankou()

{

USART1->SR&=~(1<<6);

chuankoufasongwei++;

if(chuankoufasongwei>=20){chuankoufasongge=1;

if(chuankoufasongcishu==36){

SendBuff[0]=36;

chuankoufasongshuju=Roll*572957;

chuankoufasongcishu=37;

}else

if(chuankoufasongcishu==37){

SendBuff[0]=37;

chuankoufasongshuju=Pitch*572957;

chuankoufasongcishu=38;

}else

if(chuankoufasongcishu==38){

SendBuff[0]=38;

chuankoufasongshuju=Yaw*572957;

chuankoufasongcishu=36;

}

if(chuankoufasongshuju<0){

SendBuff[chuankoufasongge]=45;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=100000000){

SendBuff[chuankoufasongge]=(chuankoufasongshuju/100000000)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=10000000){

SendBuff[chuankoufasongge]=(chuankoufasongshuju/10000000)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=1000000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1000000)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=100000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/100000)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=10000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10000)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=1000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1000)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=100) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/100)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(chuankoufasongshuju>=10) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

if(1) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1)%10+48;  // 给SBUFA数据

chuankoufasongge++;

}

for(;chuankoufasongge<20;chuankoufasongge++)

SendBuff[chuankoufasongge]=10;

chuankoufasongwei=0;

}

USART1->DR=SendBuff[chuankoufasongwei];

}

2449164582

不错，帮顶。。
希望可以共享下下

一般BDN

很厉害！！！

正点原子

谢谢分享。。。。

2449164582

谢谢分享。。。。。。。。。。。

俯身若梦

赞，太感谢楼主了，雪中送炭

344864311

楼主用的什么摄像模块

wsnfly

回复【7楼】344864311:
---------------------------------
柏通图传模块

344864311

能给个链接吗，我需要买一个。

wsnfly

回复【9楼】344864311:
---------------------------------
http://item.taobao.com/item.htm?spm=a1z09.2.9.11.YdWvN1&id=25409468806&_u=v7hq0tl0460

阿愣不说话

Mark，谢谢分享

小宇宙阿栋

mark
谢谢分享！！

z-cyounG

你好，能不能给我发一份第一个视频呢？460339847@qq.com

0055

cxk18811751176

大神你好，能给发一下第一个视频吗？？？  谢谢啦

随风飞

不错，，

garymg

楼主的电调驱动用的是哪一家的
各位大侠帮忙看看此款电调驱动怎么样
FD6288是一款集成了三个独立的半桥栅极驱动集成电路芯片，专为高压、高速驱动MOSFET和IGBT设计，可在高达+250V电压下工作。FD6288内置VCC/VBS欠压（UVLO）保护功能，防止功率管在过低的电压下工作。FD6288内置直通防止和死区时间，防止被驱动的高低侧MOSFET或IGBT直通，有效保护功率器件。FD6288内置输入信号滤波，防止输入噪声干扰

xiaomao1490

楼主你好，能给发一下第一个视频吗？  谢谢啦！邮箱：778261169@qq.com

wsnfly

xiaomao1490 发表于 2016-4-25 14:17
楼主你好，能给发一下第一个视频吗？  谢谢啦！邮箱：

是制作飞控的过程，被封了。。。为何天朝做个飞控都要和谐

空心杯

哼！天朝这里不清真啊，我就觉得该鼓励自己做飞控的

一起走过的日子

楼主，解释一下MPU6050姿态解算部分的代码吧，不懂四元素解算啊

一起走过的日子

楼主给我发个飞控自作视频呗，谢谢2336599635@qq。com

初中11班

谢谢分享！！

wsgfyw

楼主伟大，发个制作过程视频，和详细点的资料可以不，初学者太薄弱了，谢谢啦。邮箱：2820048139@qq.com

wsnfly

wsgfyw 发表于 2016-12-12 22:29
楼主伟大，发个制作过程视频，和详细点的资料可以不，初学者太薄弱了，谢谢啦。邮箱：

详细制作过程已被和谐