飞行控制算法还不是很好，今年有时间继续完善一下，先上视频看看，大家有兴趣的一起学习
! G9 A4 c5 F& G: Y+ }+ R好像不能直接放视频哦，那只能跳转到优酷了
+ a! |# S3 [5 V
一个是制作调试的视频
, B& }4 j% y/ j- i+ G5 @' S4 jhttps://v.youku.com/v_show/id_XN ... 1-103.3.1-2.1-1-1-0

这个是飞机上FPV的视频
( M% H+ z" Y8 D6 k/ V1 Ihttps://v.youku.com/v_show/id_XNzI0NDkxNTI4.html

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

学习学习，支持一下！

谢谢分享

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

不是公开资料的吗

沐紫 发表于 2015-3-4 17:03
) b- Q6 ~% S) O不错，楼主能详细介绍下吗？

! H) O# L" S- f1 B/ o0 r& t. a当然可以，就是写的太乱了怕大家看不清楚
9 p* v0 h* o. K. w9 L. J. {
0 o& k# q. ]/ S/ W! m& d5 B

#include "aquanjubianliang.c"
2 P! l3 e) N: W) I5 k! K
#include "usart.h"
* [1 r. r9 B8 B0 N#include "delay.h"
4 I5 z" h. H# x, E#include "led.h"
#include "key.h"
; \& e6 f0 V& ]1 E6 o#include "exti.h"
' L. [1 m4 z0 c& w6 y#include "wdg.h"
1 J& X3 n0 f$ F2 p#include "timer.h"
#include "lcd.h"   
#include "rtc.h"
: w) w5 e  u! A% T% [#include "wkup.h"
#include "adc.h"
#include "dma.h"
5 ]$ G8 \$ J. Q#include "24cxx.h"
#include "flash.h"
. P: w7 h- |7 J* ~: x#include "24l01.h"
#include "mmc_sd.h"
#include "remote.h"
; G4 W; E, P7 P#include "ds18b20.h"
& Q3 B! [5 B" g% w#include "mouse.h"
4 s: t. N# i* k. h5 X& b#include "text.h"
#include "fat.h"
#include "fontupd.h"
0 D& g4 H7 t! z/ P5 b4 T9 U#include "IIC.h"
#include "inv_mpu.h"
: b; Q7 U) E" l0 C#include "inv_mpu_dmp_motion_driver.h"
! `+ ^$ J9 _5 \- L#include "math.h"

zhinengchuankou();
, m* `8 l- @8 ]1 {7 ?
#define duoji1 PAout(5)// PA8
4 ?/ x  H- |: P/ |#define duoji2 PAout(1)
$ Q7 ~/ e+ f, d$ x# z#define duoji3 PAout(0)
#define duoji4 PAout(2)
* j6 J+ m' c" N
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表示最大稳定时的速度
& b0 ~6 @( G; _& X4 Yu16 jiaosuduchangshu=100,jieshoulingmin=4,shijijiaosuduchangshu2=50,zuidaxiuzhengsudu=250,zuixiaotiaozhengshudu=6,weitiaoweizhi=2,jiaosuduqudiancishu=2,zuidatiaozhengsudu=60;
6 F" B# b/ V& S; f2 C
2 W" i7 J4 f9 @
; t9 n, W) l' ]) c; V; G' R//kongzhilingmin接收机的灵敏度用
u32 kongzhilingmin=30000;

u16 jieshouji[6],wei=0,jiesuo=0,jiaosuducishu=0,shijijiaosuduchangshu=20;
8 c1 ^5 g. _9 W( y% p4 is32 h,duojikongzhi[8],i,zhongduancishu,xlilunjiaosudu,ylilunjiaosudu,zlilunjiaosudu,xpianyijiaosudu,ypianyijiaosudu,zpianyijiaosudu,xjiaosudu,yjiaosudu,zjiaosudu,lingshijiaosudu,xjsd[100],yjsd[100],zjsd[100];
1 n9 m8 _- d% ~- l. Jdouble 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};
5 y  o) f& S3 v

int main(void)
) V0 s4 K# E; B" j0 z{
& a, B7 S0 L& g/ \9 {$ Q: V$ l//    u16 i;
3 J6 C& l. _, U1 o{
, ]' {3 O7 z/ P; T6 z: |6 eduojipianyi[0]=0;
duojipianyi[1]=0;
0 y1 r8 u# J- H! ?7 hduojipianyi[2]=0;
duojipianyi[3]=0;
" ?* o& v4 g# \6 Y  duojikongzhi[0]=1100;
duojikongzhi[1]=1100;
duojikongzhi[2]=1100;
1 P2 V: a+ q' c- g0 L( R( vduojikongzhi[3]=1100;
6 k6 {, Q) y1 U$ B7 T. {% }duojikongzhi[4]=2000;
}
   Stm32_Clock_Init(9); //系统时钟设置
# w7 ]- b# U: T! J5 q6 |delay_init(72); //延时初始化
uart_init(72,1600);  //串口1初始化  
//USART1->DR=SendBuff[1];
gpioshezhi(); //引脚设置
# V! k4 j- E; p7 s% ?) @" B* f. Nled4=0;
// LED_Init();          //LED初始化
3 K3 |, u: @, s! k  v6 D: ii2cInit(); //I2C接口设置
9 P9 J" ?6 N/ u//Timer4_Init(7199,1000);
Timer3_Init(10000,72); //定时器3，中断开启，时间不定，输出PWM信号控制螺旋桨转速   抢断优先级
Timer2_Init(7199,70); //定时器2，有中断，70毫秒读取陀螺仪数据
Timer1_Init(60000,72);TIM1->CR1|=0x01;//定时器一，没有中断，用于计时接收机信号
; O( n" g0 Z8 e  R1 g6 o7 h. Q% Z5 NEXTI15_10_Init(); //10-15中断 接收机信号中断 抢断优先级
delay_ms(100); //延时300MS
chuankoufasong(36,1); //串口发送，检测芯片是否运行
. ~; _9 E& d" K, n5 C4 lresult = mpu_init(); //dmp读出
% E5 [3 b4 l! {% B% jdelay_ms(100); //延时300MS
{//去角速度偏移量，MPU6050制造误差
) Y! ^( g# r+ g$ vxpianyijiaosudu=dumpu6050jicunqi(0);
: ?3 t) C3 A& ?( T  L; _$ nypianyijiaosudu=dumpu6050jicunqi(1);
zpianyijiaosudu=dumpu6050jicunqi(2);
}

0 h9 g0 r6 e4 Sif(!result)
  {   
chuankoufasong(36,2);
8 u- ]+ y, w4 o+ @2 r( Qmpufifo_init(); //dmp——fifo设置
  }

MYDMA_Enable(DMA1_Channel4);
chuankoufasong(37,4);
$ B# n" V3 @0 `/ I0 h1 ^6 w" IUSART1->DR=SendBuff[chuankoufasongwei];
3 S1 h0 o1 O2 n6 L9 _) D6 j   while(1)
{
: A7 g, t! `( G7 x0 i& r8 O{ //计算出理论飞行角度
, Z! _) ?" Q  n/////************************************************************************//
lilunyoumen=shuruyoumen;
lilunfuyi=0.4*(shurufuyi/300);
lilunshengjiang=0.4*(shurushengjiang/300);
5 K4 t1 P4 h3 o  q( |1 H0 ~  slilunfangxiang=lilunfangxiang+nifangxiang*2 ;
nifuyi=0;
5 \$ @( C6 ~( `/ g# Y# X6 b% Fnishengjiang=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;}
" h) n$ M5 V. U  X& v2 N% L! Zif(lilunfangxiang>2*3.14159)lilunfangxiang=lilunfangxiang-2*3.14159;
& h5 G3 G+ n- L# g+ n8 tif(lilunfangxiang<0)lilunfangxiang=lilunfangxiang+3.14159*2;
, N2 s" c1 m: y& E6 Fif(lilunfuyi>3.14159)lilunfuyi=lilunfuyi-3.14159*2;
if(lilunfuyi<-3.14159)lilunfuyi=lilunfuyi+3.14159*2;
}
{ //计算出实际坐标和理论坐标差
; |/ H" t- P3 gchafuyi=Roll-lilunfuyi;
8 f' \* T0 F9 vif(chafuyi>pi)chafuyi-=2*pi;
) `8 Z9 I! l3 C8 E% v. N" xif(chafuyi<-pi)chafuyi+=2*pi;
//if(chafuyi>(pi/8))chafuyi=pi/8;
//if(chafuyi<(-pi)/8)chafuyi=-pi/8;
6 e- ]3 }) Y& Y+ S8 gchashengjiang=-(Pitch-lilunshengjiang);
if(chashengjiang>(pi))chashengjiang-=2*pi;
6 d: h, A# f* |. _8 `$ I, vif(chashengjiang<-(pi))chashengjiang+=2*pi;
//if(chashengjiang>(pi/8))chashengjiang=pi/8;
//if(chashengjiang<(-pi)/8)chashengjiang=-pi/8;
5 q& i- f. j$ }! u. Rchafangxiang=-(Yaw-lilunfangxiang);
' o" c2 @" c7 j) `1 P  Tif(chafangxiang>pi)chafangxiang-=2*pi;
if(chafangxiang<-pi)chafangxiang+=2*pi;
: C- E4 `% g. N4 }- H) q  {( R4 }, G//if(chafangxiang>(pi/8))chafangxiang=pi/8;
0 N, V3 g+ H! [& `7 i//if(chafangxiang<(-pi)/8)chafangxiang=-pi/8;
}
) j( G& w/ o- M8 w9 I5 t{ //计算出飞行器各个轴需要的理论角速度
" q" h& S: g7 Z! f% axlilunjiaosudu=(chafuyi )*jiaosuduchangshu;
  V; z8 W7 u9 z; H6 B& i" Zylilunjiaosudu=(chashengjiang*cos(Roll)+chafangxiang*sin(Roll))*jiaosuduchangshu;
5 {( D: ^; z# F1 A* `) |# @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;
. x1 Y5 j: K# p4 ]+ [- d* \3 Aif(xlilunjiaosudu<zuixiaotiaozhengshudu&&xlilunjiaosudu>weitiaoweizhi)xlilunjiaosudu=zuixiaotiaozhengshudu;
if(ylilunjiaosudu<zuixiaotiaozhengshudu&&ylilunjiaosudu>weitiaoweizhi)ylilunjiaosudu=zuixiaotiaozhengshudu;
  i% n# B- Z' F5 O8 o/ \if(zlilunjiaosudu<zuixiaotiaozhengshudu&&zlilunjiaosudu>weitiaoweizhi)zlilunjiaosudu=zuixiaotiaozhengshudu;
if(xlilunjiaosudu>-zuixiaotiaozhengshudu&&xlilunjiaosudu<-weitiaoweizhi)xlilunjiaosudu=-zuixiaotiaozhengshudu;
  A2 m, q' {3 ?$ Q/ m$ dif(ylilunjiaosudu>-zuixiaotiaozhengshudu&&ylilunjiaosudu<-weitiaoweizhi)ylilunjiaosudu=-zuixiaotiaozhengshudu;
  s, \; _( }3 W4 b7 kif(zlilunjiaosudu>-zuixiaotiaozhengshudu&&zlilunjiaosudu<-weitiaoweizhi)zlilunjiaosudu=-zuixiaotiaozhengshudu;
: F; z. ?3 B0 E1 L- c& o0 u/ N, h}
& x0 a7 \, M, O, x/ F& h{   //计算角速度平均数

  ~! e' W$ p* V: C* Q6 k3 _" jxjiaosudu=0;
7 a1 m' a9 {6 \$ H# G8 pyjiaosudu=0;
zjiaosudu=0;
for(i=0;i<jiaosuduqudiancishu;i++)
{
9 V1 e, q1 u+ y/ v8 ixjiaosudu+=xjsd[i];
% H2 H8 O3 C1 Z5 m/ H+ Xyjiaosudu+=yjsd[i];
) A1 ?0 c0 o% a/ g* h9 }, v" Vzjiaosudu+=zjsd[i];
}
xjiaosudu/=jiaosuduqudiancishu;
5 G- a1 }' i+ ~/ |/ b9 @yjiaosudu/=-jiaosuduqudiancishu;
zjiaosudu=zjiaosudu/jiaosuduqudiancishu;
. |2 @8 v9 ~2 U: L}

& `# a3 e. w% i! @6 Z( p( |
, R" ~6 c- F0 s. Vif(shuruyoumen>1200) //如果油门处于激活状态
- t5 H5 x3 l1 f% x3 [! Q' c8 Y/ U' D{ //从理论角速度计算功电机运行的信号强度
$ K2 P% V; W) X. r
5 D# m0 i/ q+ O' x7 Kshijijiaosuduchangshu=shijijiaosuduchangshu2*(lilunyoumen-800)/1000*(lilunyoumen-600)/1000;
lingshijiaosudu=(-xjiaosudu)/shijijiaosuduchangshu;
if(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;
# r6 v. e! n# j. jif(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;
8 |! U9 y3 t. Y9 O5 |( l7 H//xjiaosudu=-shurufuyi/jiaosuduchangshu-lingshijiaosudu;//角速度调整模式时 jiaosuduchangshu=15
//xlilunjiaosudu=-shurufuyi/jiaosuduchangshu;
. X& K; e$ u: m! U6 S8 O1 Exjiaosudu=xlilunjiaosudu-lingshijiaosudu;
lingshijiaosudu=(yjiaosudu)/shijijiaosuduchangshu;
$ e# r. e4 r/ i2 Iif(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;
if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;
//yjiaosudu=shurushengjiang/jiaosuduchangshu-lingshijiaosudu;
- x% |1 C; D( {, `% A/ i//ylilunjiaosudu=shurushengjiang/jiaosuduchangshu;
yjiaosudu=ylilunjiaosudu-lingshijiaosudu;
: J1 e  ^: |! c/ R0 klingshijiaosudu=(zjiaosudu)/shijijiaosuduchangshu;
4 d! l  h1 w8 tif(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;
if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;
//zjiaosudu=shurufangxiang/jiaosuduchangshu-lingshijiaosudu;
* z+ a) `& k9 b9 C% z. j" m- K//zlilunjiaosudu=shurufangxiang/jiaosuduchangshu;
) t2 Z5 H# x+ i' G9 i' b$ f, Wzjiaosudu=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];
3 L4 W- v6 @2 ]0 Lduojikongzhi[2]=lilunyoumen-xjiaosudu-yjiaosudu+zjiaosudu*3+duojipianyi[2];
duojikongzhi[3]=lilunyoumen+xjiaosudu-yjiaosudu-zjiaosudu*3+duojipianyi[3];
5 E7 N6 X$ {& a3 D2 x8 g) s7 zfor(i=0;i<4;i++)
{
if(duojikongzhi[i]<1200)duojikongzhi[i]=1200;
}
  s5 P' F% d) W; {}
' j8 d# i) |( K9 S3 E* F. Velse { //如果油门为0
duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=1100;
% y5 @$ G4 B% `/ d# H! X+ Alilunfuyi=Roll;
lilunshengjiang=Pitch;
lilunfangxiang=Yaw;
" a  f7 M2 v% s, w% ^}
//t=double(jieshouji[0])/100;
/*
for(i=0;i<6;i++){
. }, T' Y4 D8 i}
chuankoufasong(36,shurufuyi);
4 L. S$ J% C* x4 I5 [, v8 e% i7 T9 j. Ychuankoufasong(37,shurushengjiang);
, R, ?7 n0 W. i" k- W- vchuankoufasong(38,shuruyoumen);
chuankoufasong(39,shurufangxiang);delay_ms(30);
{ //发送串口数据，功电脑读取.
3 C+ r: Z2 E3 T$ sh=shurufangxiang;
chuankoufasong(36,xjiaosudu); delay_ms(10);
h=shuruyoumen;
- @2 A- Z3 [" H! H1 echuankoufasong(37,yjiaosudu); delay_ms(10);
h=chafangxiang*57;
chuankoufasong(38,zjiaosudu);
delay_ms(10);
chuankoufasong(39,duojikongzhi[3]);
$ J$ o: f  J/ v) M8 N9 Jdelay_ms(10);
//duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=shuruyoumen+1100;
}
h=xjiaosudu;
5 }6 G' w. C3 N- x% kchuankoufasong(36,h);
h=yjiaosudu;
chuankoufasong(37,h); delay_ms(10);
1 Y. i( D% k* `2 f$ a0 g; _3 ph=zjiaosudu;
9 d" T6 D2 B' J! A+ ]" Jchuankoufasong(38,h);
h=shurufangxiang;
. @0 y# t! s" `3 Q+ I$ r: Q" mchuankoufasong(39,h);
& w" S6 Q) [& U+ t# C: j4 Ih=shuruyoumen;
2 j  w' c( j3 O( hchuankoufasong(39,jieshouji[4]);
: E! \' m% i( g' sh= dumpu6050jicunqi(0);
4 d( `! p6 C- |+ G6 ]chuankoufasong(37,h);
2 P$ V5 {- d4 ?& q5 D1 z% wh= dumpu6050jicunqi(1);
chuankoufasong(38,h);
h= dumpu6050jicunqi(2);
* F6 Y' j/ k5 e* n, d9 |6 Kchuankoufasong(39,h);
) {0 A. y; J4 s, bchuankoufasong(36,jieshouji[3]);
chuankoufasong(37,jieshouji[2]);
chuankoufasong(38,jieshouji[4]);
*/
* ^$ q! Z- f1 U, z( x8 b* p{
5 O) ]  c' V; p2 @2 R8 `if((USART1->SR&0X40)!=0)
5 e1 Y' v2 ]. U4 a, S$ z3 y2 U{

/ }  J. C7 V# E, H, K, s1 i7 Dzhinengchuankou();
: x' n3 G' Q# o! P: M  a% u}
}

6 U5 I% L1 W* O4 t/ F1 o}

}
5 N- u: u- \" e2 jvoid TIM2_IRQHandler(void) //定时器2 ，用于读取DMP数据
{
/ n; S. k2 C& q# F/ E& c; x/ d. ZTIM2->SR&=~(1<<0);//清除中断标志位                  
2 B/ t0 U' b" k6 ~, O+ V+ i  
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);  
     if (sensors & INV_WXYZ_QUAT )
{
' G1 [, w4 F0 I   q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
: S7 N) r) w) ^: ?) o; t$ F 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) ;//方向
0 x. @# ^+ E, N3 v" A$ x
; ]" u: m9 {, K: v //h=Roll*57295;
& |0 i: ]( d0 D' [7 d //chuankoufasong(36,h);
5 P4 s) p9 g0 Y2 u //h=Pitch*57295;
1 `+ q0 ~5 A6 t% G# l% E //chuankoufasong(37,h);
//h=Yaw*57295;
// chuankoufasong(38,h);
* K$ }- ^' F- s6 f0 Z: B4 y6 d. U
" Q7 `; o, |  J2 I: p// PrintChar("nihao\n");
}

+ m) K; F, a4 h- R2 R3 g8 F7 g+ Mxjsd[jiaosuducishu]=(gyro[0]-0);
" u" @# A3 p2 x1 j# Z, D4 qyjsd[jiaosuducishu]=-(gyro[1]-0);
zjsd[jiaosuducishu]=(gyro[2]-0);
jiaosuducishu++;
+ _0 I/ `1 b  Z# I- Y- j+ M, ]+ g. Kif(jiaosuducishu>=jiaosuduqudiancishu)jiaosuducishu=0;

}

. P$ K6 z0 G: ?0 P, `8 a
$ T  I9 b3 d; w5 o2 [9 }* W# z+ ]


- P8 ^5 d1 _7 j; Q8 N0 E
! f6 O4 W: R& G# L
5 r+ q, @& E# @6 p' f

- w8 x' |+ w% u! C5 c; r

void EXTI15_10_IRQHandler(void) //接收机信号接收中断         耗时2微秒  以内
{
3 C5 z: J, x% Q% H- N- a) Bu8 jicixunhuan;
for(jicixunhuan=12;jicixunhuan<16;jicixunhuan++)
" p; b6 e" s- b  ^7 @* X! o{
if(((GPIOB->IDR>>jicixunhuan)&1)){
shuruyinjiao=jicixunhuan;
TIM1->CNT=0;
}
, y8 ~1 [4 J6 G}

: Y+ z6 b% |- x' A& ]if(!(GPIOB->IDR>>12&0xf)){ //如果接收机发送结束
if(TIM1->CNT>1000&&TIM1->CNT<2000)
& \/ Z7 L- }9 P4 R1 J{
jieshouji[shuruyinjiao-10]=TIM1->CNT; //将接收机变量保存到变量
8 y, v; d# ~1 k) R0 `. Ajieshouji[shuruyinjiao-10]/=jieshoulingmin;
jieshouji[shuruyinjiao-10]*=jieshoulingmin;
}
if(shuruyinjiao==15)
{
# V0 Q; Q* j5 |- D8 l9 B7 U/////************************************************************************//
//计算得到每个摇杆的行程
if((youmen[0]-jieshouji[3])<100&&(shuruyoumen-jieshouji[3])<100)
& k. o  @3 R8 ?1 c  M& U6 V{
& b( X+ f  m2 d; qshurufuyi=1492-jieshouji[3];
}
youmen[0]=jieshouji[3];
shurushengjiang=1492-jieshouji[2];
1 x. u' t( h% dshurufangxiang=1492-jieshouji[5];
shuruyoumen=jieshouji[4];
/////************************************************************************//
//每个遥感的行程转化成每次的偏移量
$ [# U  R, a5 K6 w3 enifuyi=nifuyi+shurufuyi/kongzhilingmin;
% U  b! c, e+ d# \) R; c" d: [nishengjiang=nishengjiang+shurushengjiang/kongzhilingmin;
nifangxiang=nifangxiang+shurufangxiang/kongzhilingmin;
}
1 E6 ?/ f, `5 s8 p# m}
if(shuruyoumen<1190){jiesuo++;
if(shurufangxiang>200){
$ ^  |* R# D8 M/ A  Uif(jiesuo>700) {
' u% ^1 W/ z* K) y/ r& }% A0 N5 jjiesuo=0;
suo=0;
9 ]' c* R8 ^3 c2 i0 J* B$ t}
) i  @# H% ]; H0 w0 H3 d}
3 K1 {/ {9 h5 [if(shurufangxiang<-200){
" M- T5 y" n+ Zif(jiesuo>700){
jiesuo=0;
$ [, }7 _, @! zsuo=1;
) o" k* _4 \# H- ]( _) d}
}
}
else jiesuo=0;
6 V+ i2 e0 T  s3 a: U- y! h7 Yzhongduancishu++;
* [8 a" ~$ l. K3 pEXTI->PR=1<<10;
+ x: r5 T4 F: ]* wEXTI->PR=1<<11;
: E  i1 L, p; \- P* Y% Z* nEXTI->PR=1<<12;
EXTI->PR=1<<13;
/ Z7 O! O2 Y* w& T! `9 k6 PEXTI->PR=1<<14;
; O, F! O6 {% B: S5 b- c: TEXTI->PR=1<<15;
}
. d1 m( Z* [- O2 \; X
: [( X: F7 l5 c. {1 l+ \void TIM3_IRQHandler(void) //定时器3，用于输出PWM信号   耗时1微秒 以内
{
3 [8 s5 O+ Y1 |0 w# Z& P2 A) }7 `if(suo==1){
$ y1 M) ~6 i. I( U8 A/ C: bif(shuruyoumen<1200)duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=1100;
; |4 L7 b( U; X" S5 A- }9 zTIM3->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;}
3 q6 J% d# `9 \1 ]* x$ j, |if(wei==3){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji4=1;led4=!led4;}
, i) r: C, V# ]) f4 u0 ^if(wei==4){duoji1=0;duoji2=0;duoji3=0;duoji4=0; }
# L' a7 u* d- I2 T  h$ e               

# K8 H# S) Y5 b0 Pwei++;
if(wei>4)wei=0;
/ n* `+ M4 R. I1 F& f}
if(suo==0)led4=0;
9 \) ]9 O5 M" n" E7 V. j9 G( qTIM3->SR&=~(1<<0);//清除中断标志位  
}

void TIM4_IRQHandler(void)
6 I( K8 o6 L( M8 K, F{
TIM4->SR&=~(1<<0);//清除中断标志?
/*?
! t3 c4 D8 R' D$ t  F
$ r% r  `5 I& @  U*/
}
6 i: T, j3 R9 i+ U! K

zhinengchuankou()
{
USART1->SR&=~(1<<6);
5 }3 x2 k9 F" E7 l3 }chuankoufasongwei++;
, K+ f7 x, j) w8 E4 Z- B  Yif(chuankoufasongwei>=20){chuankoufasongge=1;
: ~7 |" d4 D6 W) I" ?' [if(chuankoufasongcishu==36){
7 C! O+ b- U" c% s( `SendBuff[0]=36;
chuankoufasongshuju=Roll*572957;
chuankoufasongcishu=37;
1 M, X1 h  j2 f, K1 M$ l% ^}else
) {  r0 ?! ]8 z- S, iif(chuankoufasongcishu==37){
SendBuff[0]=37;
' L1 x& A, U1 R; I. Ychuankoufasongshuju=Pitch*572957;
/ `5 b# b0 c' @chuankoufasongcishu=38;
8 j2 e' j' e; w+ C}else
if(chuankoufasongcishu==38){
5 ?% s3 u' [5 X% \6 E0 b3 ISendBuff[0]=38;
# F: i2 }1 p3 J+ Mchuankoufasongshuju=Yaw*572957;
( u1 n7 N2 q/ a# zchuankoufasongcishu=36;
}
( j% T- _4 F' V7 q7 P7 h: Iif(chuankoufasongshuju<0){
SendBuff[chuankoufasongge]=45;  // 给SBUFA数据
8 u1 |9 a& D0 i( a) Rchuankoufasongge++;
8 p6 |0 H8 p) E4 ]}
% N5 M2 q  E" g4 Z5 H
if(chuankoufasongshuju>=100000000){
3 d1 t4 l4 ]4 A8 H0 USendBuff[chuankoufasongge]=(chuankoufasongshuju/100000000)%10+48;  // 给SBUFA数据
5 d+ T. E9 V3 u* o3 O8 Bchuankoufasongge++;
}
if(chuankoufasongshuju>=10000000){
9 L( N0 c6 F1 I, c* Y( RSendBuff[chuankoufasongge]=(chuankoufasongshuju/10000000)%10+48;  // 给SBUFA数据
chuankoufasongge++;
}
+ _/ a2 {& T6 v1 B% a* tif(chuankoufasongshuju>=1000000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1000000)%10+48;  // 给SBUFA数据
chuankoufasongge++;
}
: q6 A+ C1 Z) V+ e5 r3 ~if(chuankoufasongshuju>=100000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/100000)%10+48;  // 给SBUFA数据
chuankoufasongge++;
3 W' N! `# T1 L) U. U}
9 U" M2 h* ]  |- aif(chuankoufasongshuju>=10000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10000)%10+48;  // 给SBUFA数据
chuankoufasongge++;
: t3 f$ T0 j; r}
  U1 }7 K3 X  Q/ N3 `% `2 ~if(chuankoufasongshuju>=1000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1000)%10+48;  // 给SBUFA数据
' L) ^8 X3 B5 G9 }3 p8 |3 ~chuankoufasongge++;
}
if(chuankoufasongshuju>=100) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/100)%10+48;  // 给SBUFA数据
) K5 b- [  U7 t0 ochuankoufasongge++;
}
if(chuankoufasongshuju>=10) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10)%10+48;  // 给SBUFA数据
chuankoufasongge++;
}
if(1) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1)%10+48;  // 给SBUFA数据
* d! z1 p# w7 R  t5 L+ a4 zchuankoufasongge++;
, U/ v: k/ X% g& y}
for(;chuankoufasongge<20;chuankoufasongge++)
SendBuff[chuankoufasongge]=10;
chuankoufasongwei=0;
7 x+ j7 J- O8 Q8 Y}
; y) w$ G9 G% l$ @9 x: b& N7 s" [! FUSART1->DR=SendBuff[chuankoufasongwei];
}

这些代码仔细看看不错

foxglove 发表于 2015-3-13 14:59
* I- U  ~# X8 z4 j& B0 u1 ~这些代码仔细看看不错

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