飞行控制算法还不是很好，今年有时间继续完善一下，先上视频看看，大家有兴趣的一起学习
好像不能直接放视频哦，那只能跳转到优酷了

一个是制作调试的视频
https://v.youku.com/v_show/id_XN ... 1-103.3.1-2.1-1-1-0

这个是飞机上FPV的视频
https://v.youku.com/v_show/id_XNzI0NDkxNTI4.html

你这是什么意思？这样的贴子，直接删除了。

学习学习，支持一下！

谢谢分享

不错，楼主能详细介绍下吗？

不是公开资料的吗

沐紫 发表于 2015-3-4 17:03
不错，楼主能详细介绍下吗？

当然可以，就是写的太乱了怕大家看不清楚

#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[i];
yjiaosudu+=yjsd[i];
zjiaosudu+=zjsd[i];
}
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[i]<1200)duojikongzhi[i]=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副翼
Pitch  = 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->PR=1<<10;
EXTI->PR=1<<11;
EXTI->PR=1<<12;
EXTI->PR=1<<13;
EXTI->PR=1<<14;
EXTI->PR=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];
}

这些代码仔细看看不错

foxglove 发表于 2015-3-13 14:59
这些代码仔细看看不错

我写的乱七八糟的，很长时间没看，自己都看的累