你的浏览器版本过低,可能导致网站不能正常访问!
为了你能正常使用网站功能,请使用这些浏览器。

去年做的一个飞控,STM32F103C8T6

[复制链接]
wsn-412377 发布时间:2015-3-4 12:19
飞行控制算法还不是很好,今年有时间继续完善一下,先上视频看看,大家有兴趣的一起学习
% v( P( |- M1 M! ?% ^5 ^好像不能直接放视频哦,那只能跳转到优酷了
+ z: @% G7 P/ y
" ^4 B4 H/ |1 r. x3 k, J$ Y一个是制作调试的视频$ m- Z# J; ]* b, d% ]2 _. M2 v; Z
https://v.youku.com/v_show/id_XN ... 1-103.3.1-2.1-1-1-0
  x( k+ B0 w+ n( d( b1 h4 F4 X0 t3 U% I! x3 o
这个是飞机上FPV的视频
; t4 G! {) R& }/ h% mhttps://v.youku.com/v_show/id_XNzI0NDkxNTI4.html! n, f% `+ R$ c- ]) M
赞 收藏 评论10 发布时间:2015-3-4 12:19

举报

10个回答
党国特派员 回答时间:2015-3-4 13:01:21
你这是什么意思?这样的贴子,直接删除了。
天使♀的☆宇★翼 回答时间:2015-3-4 15:56:28
学习学习,支持一下!
feel-376797 回答时间:2015-3-4 15:59:52
谢谢分享
拼命三郎 回答时间:2015-3-4 16:27:23
xxxx.png
沐紫 回答时间:2015-3-4 17:03:45
不错,楼主能详细介绍下吗?
北斗光寒 回答时间:2015-3-4 20:49:53
不是公开资料的吗
wsnfly~ 回答时间:2015-3-13 09:41:14
沐紫 发表于 2015-3-4 17:03
6 [/ x+ s' H# `( ?$ ]6 [不错,楼主能详细介绍下吗?

# h3 U9 Q( V1 D& f) r  u6 s当然可以,就是写的太乱了怕大家看不清楚. v6 [* q" @/ ]0 I

$ y& L# E3 H) ^2 M5 w
wsnfly~ 回答时间:2015-3-13 14:36:34
#include "aquanjubianliang.c"+ l/ B5 n' f5 ]) n7 P; v& {

( C% c8 M8 i# Q& p2 w#include "usart.h"
& k' x$ V* M# M1 a#include "delay.h"
5 `9 I" _2 ?/ Q' M#include "led.h"
% o1 ^: p, B! k) k#include "key.h"
; m8 ]7 x$ L2 K' _* A$ J3 X. o#include "exti.h"/ a. |. P" u* [
#include "wdg.h"8 E4 o# Q0 c& Q6 n0 u
#include "timer.h". s) r- J& n# y& N% i
#include "lcd.h"   
8 {! M* I. j) O$ ?( t; a/ m9 w) L#include "rtc.h"
( f$ u5 h$ r/ V2 ]0 }, g#include "wkup.h"
8 q) [4 a. _+ f) f  R#include "adc.h"" g, i$ c5 v# u  w" v* a
#include "dma.h"3 d$ J6 J$ t$ L- R3 F
#include "24cxx.h"
5 S5 R9 P! t8 N+ b#include "flash.h"
2 u3 T2 e& E  l! x#include "24l01.h"# I- b& A+ a1 Z; E
#include "mmc_sd.h"
' ~1 H4 |# f& p: C2 g#include "remote.h"! J" g5 U2 N; W+ g' t
#include "ds18b20.h"
8 H1 E) m; W1 t#include "mouse.h"
1 e5 T7 m6 y8 a8 }" \#include "text.h"
7 D1 M2 J6 }  M  J#include "fat.h"2 ]+ I" g5 c6 ~% ~. f  S
#include "fontupd.h"$ N" @8 K3 q6 U. n
#include "IIC.h", a. H" p/ O6 @& }
#include "inv_mpu.h"5 k: d  U3 G$ Z2 j
#include "inv_mpu_dmp_motion_driver.h", g4 R' Z% p; j5 S# G
#include "math.h"4 I" o! D8 x* g3 I, K& s+ e( v

& j0 S- B; w: I- ~" V1 B* }6 j+ W8 vzhinengchuankou();
! S( F8 t+ Y2 [0 u# ]  M3 H% |
9 J( Q) ~/ R4 B. w#define duoji1 PAout(5)// PA8
  |" }1 y) @" N. u* j#define duoji2 PAout(1)
! T. U8 u+ h; G, Q/ y) a; @; E  a#define duoji3 PAout(0)* a. B) g3 T6 W  I7 k+ g
#define duoji4 PAout(2)5 }% ?" D# x" j; M$ O4 P

: X" \6 B* j( d( `! T) B& k. Us16 duojipianyi[4];
+ A; D0 W3 d" K6 R- A
  Y1 q; J( Q5 |6 V4 a: o& T//u16 jiaosuduchangshu=50,jieshoulingmin=4,shijijiaosuduchangshu=50,zuidaxiuzhengsudu=60,zuixiaotiaozhengshudu=10,weitiaoweizhi=10,jiaosuduqudiancishu=5,zuidatiaozhengsudu=40;//可用
! L/ G0 C% H# H: C" u8 Q( T3 J4 E* l2 |" x2 W7 e
//jiaosuduchangshu计算出各轴需要的理论角速度用;jieshoulingmin由于接收机定时器有误差,消除误差用,会消除精度; shijijiaosuduchangshu用于取得角速度之后除数;zuidaxiuzhengsudu自稳系统的最大旋转速度; W8 ^/ u5 I' w, X3 E1 v/ M! k0 q
//zuixiaotiaozhengshudu当调整速度小于此数值时,增加调整速度,weitiaoweizhi当调整速度小于此数值时 进行微调就可以,jiaosuduqudiancishu角速度的次数累加,越多越准确,但反应越慢,zuidatiaozhengsudu表示最大稳定时的速度% R9 e  j+ V, h! i2 ]  P  H
u16 jiaosuduchangshu=100,jieshoulingmin=4,shijijiaosuduchangshu2=50,zuidaxiuzhengsudu=250,zuixiaotiaozhengshudu=6,weitiaoweizhi=2,jiaosuduqudiancishu=2,zuidatiaozhengsudu=60;
8 m8 p) h7 R, d( ]4 {4 M# X4 K% c/ b4 N1 G! u
+ T+ [9 M! N, e+ c+ j  H2 {& j% g+ S
//kongzhilingmin接收机的灵敏度用$ q, [5 t- \- e
u32 kongzhilingmin=30000;
5 }4 B( ^0 e2 p' x8 N
/ k  L2 R; g- ]* H  q: Zu16 jieshouji[6],wei=0,jiesuo=0,jiaosuducishu=0,shijijiaosuduchangshu=20;
( @7 Y& ^/ o6 n- q: j. ~) X# Xs32 h,duojikongzhi[8],i,zhongduancishu,xlilunjiaosudu,ylilunjiaosudu,zlilunjiaosudu,xpianyijiaosudu,ypianyijiaosudu,zpianyijiaosudu,xjiaosudu,yjiaosudu,zjiaosudu,lingshijiaosudu,xjsd[100],yjsd[100],zjsd[100];
2 k; o9 Q% X7 w$ Wdouble shurufuyi,shurushengjiang,shurufangxiang,shuruyoumen,t,pi=3.14159,youmen[10];+ s& {- |% N: K8 j
double nifuyi=0,nishengjiang=0,nifangxiang=0,niyoumen=0;
* {- [# d/ ~$ Wdouble lilunfuyi,lilunshengjiang,lilunfangxiang,lilunyoumen,chafuyi,chashengjiang,chafangxiang,chayoumen,shijiyoumen;; l( c' q7 m. P: P) P8 {6 n
u8 shuruyinjiao=10,suo=0,SendBuff[256]={97};
( w: M8 [' K8 K1 S. A$ |/ L8 f+ R' k( q6 S$ i# A6 I# v
: g: I  o5 H1 }% M3 L+ F+ v
int main(void)
) U( T& ?- I0 q  D{$ l& w6 D% i" ^$ h5 g: d
//    u16 i;
9 Q1 ?# v( n( \' }) E{
: P$ t; L1 L* ]0 {9 A0 nduojipianyi[0]=0;0 C( o) W( G8 T. a
duojipianyi[1]=0;
# v6 D! Y& M$ N$ S# _2 g2 b3 q6 Zduojipianyi[2]=0;
- t4 R/ e# x3 Q9 B5 d9 g- cduojipianyi[3]=0;
2 s; s+ f: Y) w3 _7 ^! |  duojikongzhi[0]=1100;
& d" \4 d1 z3 V# Y$ y$ t% _! Jduojikongzhi[1]=1100;' @: B. S" B5 u$ Z. d" y# N1 m
duojikongzhi[2]=1100;8 b8 q9 t8 y( O) q3 D
duojikongzhi[3]=1100;
7 x: ~/ ^( k: e, G( R- R8 {duojikongzhi[4]=2000;
7 Z+ |  }) o- l4 f$ w- A}/ L& N" @1 g7 Y, k7 H& b
   Stm32_Clock_Init(9); //系统时钟设置
! T% G2 x% l, x6 ^+ {# Kdelay_init(72); //延时初始化
/ Q& U9 |" g: g. c. q+ tuart_init(72,1600);  //串口1初始化  ' v$ Q4 {* y9 k( w* n' h- |" D* ^2 }8 M
//USART1->DR=SendBuff[1];
1 R: [: a) r  v5 W" Qgpioshezhi(); //引脚设置1 E4 H) @* ^) ?, l5 G3 ^+ M& D
led4=0;
' t& P: J, B0 M% Q// LED_Init();          //LED初始化9 J( ]% A) o/ e+ \% x$ m: A( ~
i2cInit(); //I2C接口设置
1 L$ `& i" a; y1 f; p- v//Timer4_Init(7199,1000);
. `- t2 j8 c+ D- V3 m- qTimer3_Init(10000,72); //定时器3,中断开启,时间不定,输出PWM信号控制螺旋桨转速   抢断优先级
) l7 S4 R" D. C, A; V4 @Timer2_Init(7199,70); //定时器2,有中断,70毫秒读取陀螺仪数据
, i2 t6 f/ M% T! }9 Y/ }6 vTimer1_Init(60000,72);TIM1->CR1|=0x01;//定时器一,没有中断,用于计时接收机信号- K, |3 o' i, `- E2 E4 [
EXTI15_10_Init(); //10-15中断 接收机信号中断 抢断优先级3 A+ P! j, i) s; w8 I2 M: j
delay_ms(100); //延时300MS
6 n4 r9 I( d# c1 M1 Uchuankoufasong(36,1); //串口发送,检测芯片是否运行: W, m9 A9 v" R, g
result = mpu_init(); //dmp读出/ {# v" y% v: [! }; O7 N6 W1 x6 j
delay_ms(100); //延时300MS  b. O) }9 v. c' }0 e6 q* X6 f
{//去角速度偏移量,MPU6050制造误差
5 N/ z' J/ n. Qxpianyijiaosudu=dumpu6050jicunqi(0);
6 U" D. m! f, G( S# c" D6 sypianyijiaosudu=dumpu6050jicunqi(1);
  p  h, Z* z4 Szpianyijiaosudu=dumpu6050jicunqi(2);2 c# y' R( U% T5 l+ J
}
) n: t# Z" m3 R4 F
+ z) Q+ t2 g+ ^* R3 K. }3 W  rif(!result)0 |$ G5 _, J& x9 F: J+ v
  {   
9 K# w7 d  k. schuankoufasong(36,2);
/ ^% X8 b) A4 G# }mpufifo_init(); //dmp——fifo设置* H$ p# _+ t3 p1 |( W2 n- l4 a
  }& }6 u$ _& B4 j& t" p- G
# l5 c% @* c0 T
MYDMA_Enable(DMA1_Channel4);
' |3 y% g! j" ^9 lchuankoufasong(37,4);+ x5 Y  M, a( I+ @4 h
USART1->DR=SendBuff[chuankoufasongwei];
" t4 I0 E6 c! f- p9 \' D9 g8 Q   while(1) ! {$ J8 M" \: x
{# F; ?: K2 {  M3 x
{ //计算出理论飞行角度% N8 s  F  C7 G" A" k3 T0 j1 v* m
/////************************************************************************//
  N! E8 v6 x7 S) G" m7 O  Flilunyoumen=shuruyoumen;
4 c6 g2 B) _/ P  Qlilunfuyi=0.4*(shurufuyi/300);
, C1 t+ f- V. ?% ulilunshengjiang=0.4*(shurushengjiang/300);
, I+ e- @* n3 U& V  @# M0 glilunfangxiang=lilunfangxiang+nifangxiang*2 ;7 A, b. D) E  c4 |- A1 `, }* @! g! v
nifuyi=0;: b0 O( |0 e8 Y) z: `9 R
nishengjiang=0;
3 a  s, }1 v  f: pnifangxiang=0;
  l* `$ y; {2 F( P+ P: a: }7 pif(lilunshengjiang>3.14159/2){lilunfuyi=lilunfuyi+3.14159;lilunfangxiang=lilunfangxiang-3.14159;lilunshengjiang=3.1415/2;}! O6 V! B" j" Y
if(lilunshengjiang<-3.14159/2){lilunfuyi=lilunfuyi+3.14159;lilunfangxiang=lilunfangxiang-3.14159;lilunshengjiang=-3.1415/2;}
5 l4 M# l6 P# f3 R9 k# k! F& k  Uif(lilunfangxiang>2*3.14159)lilunfangxiang=lilunfangxiang-2*3.14159;) r+ w% O$ G/ i/ I5 j: m. }  B- I
if(lilunfangxiang<0)lilunfangxiang=lilunfangxiang+3.14159*2;
5 F* r7 ]* _0 E! c( f0 Y, Sif(lilunfuyi>3.14159)lilunfuyi=lilunfuyi-3.14159*2;
. p$ q' s6 o. O6 [2 ]if(lilunfuyi<-3.14159)lilunfuyi=lilunfuyi+3.14159*2;
1 Y9 p. g6 u/ j& f% S# }$ }! v}: ^( d: W& U8 Q6 Y, D! ^1 V
{ //计算出实际坐标和理论坐标差
) `' }0 a7 N% ?  }0 @chafuyi=Roll-lilunfuyi;3 o1 Q" o8 z9 n7 `* `
if(chafuyi>pi)chafuyi-=2*pi;
" Y9 P! u$ ~# d" W4 }8 s! @6 \2 R) Vif(chafuyi<-pi)chafuyi+=2*pi;# `( E' o' y) ^: R( T
//if(chafuyi>(pi/8))chafuyi=pi/8;
0 r; Y4 i+ l( K- J//if(chafuyi<(-pi)/8)chafuyi=-pi/8;3 `. `* D1 ~) ?2 i; D
chashengjiang=-(Pitch-lilunshengjiang);
  _3 S% E* d, ]' J; kif(chashengjiang>(pi))chashengjiang-=2*pi;
  W5 W, j' P- R  n3 D; X8 X% C8 aif(chashengjiang<-(pi))chashengjiang+=2*pi;  L' {# {5 @/ |' v
//if(chashengjiang>(pi/8))chashengjiang=pi/8;( `5 f2 U6 t# c- |$ G+ J- f2 G
//if(chashengjiang<(-pi)/8)chashengjiang=-pi/8;
+ M/ c( N* H7 T6 Kchafangxiang=-(Yaw-lilunfangxiang);
4 h" |  s: }9 _: jif(chafangxiang>pi)chafangxiang-=2*pi;- H3 m; w: K" c( L0 o
if(chafangxiang<-pi)chafangxiang+=2*pi;
! Z! g$ |1 L) j, c: d1 g' C$ u; Q//if(chafangxiang>(pi/8))chafangxiang=pi/8;8 y. ?) p  e6 G8 r
//if(chafangxiang<(-pi)/8)chafangxiang=-pi/8;
7 v! }; W8 t' j0 v+ ~$ }! h}
6 |! F& W8 d) M5 }6 w! j3 b* l{ //计算出飞行器各个轴需要的理论角速度
4 d: y3 {4 }+ b' @3 q% G. pxlilunjiaosudu=(chafuyi )*jiaosuduchangshu;/ P& @% e: ]% h) B4 \5 K/ z, x
ylilunjiaosudu=(chashengjiang*cos(Roll)+chafangxiang*sin(Roll))*jiaosuduchangshu;/ A) c9 e$ \3 M; x
zlilunjiaosudu=(chafangxiang*cos(Roll)-chashengjiang*sin(Roll))*jiaosuduchangshu;
4 C0 N) h! }2 vif(xlilunjiaosudu>zuidatiaozhengsudu)xlilunjiaosudu=zuidatiaozhengsudu;: c( F. C1 h2 o( n: B
if(xlilunjiaosudu<-zuidatiaozhengsudu)xlilunjiaosudu=-zuidatiaozhengsudu;
8 U4 X- u4 L1 `7 B" x4 a: Nif(ylilunjiaosudu>zuidatiaozhengsudu)ylilunjiaosudu=zuidatiaozhengsudu;7 z) I- L# K6 _- a6 i
if(ylilunjiaosudu<-zuidatiaozhengsudu)ylilunjiaosudu=-zuidatiaozhengsudu;
) K' J- u. ]' Tif(zlilunjiaosudu>zuidatiaozhengsudu)zlilunjiaosudu=zuidatiaozhengsudu;
& q% n" N4 E4 _& }$ O0 x$ `2 Eif(zlilunjiaosudu<-zuidatiaozhengsudu)zlilunjiaosudu=-zuidatiaozhengsudu;" Q8 {$ z" A' r1 v: i
if(xlilunjiaosudu<zuixiaotiaozhengshudu&&xlilunjiaosudu>weitiaoweizhi)xlilunjiaosudu=zuixiaotiaozhengshudu;
6 M# V7 p0 u) |' S9 L9 Tif(ylilunjiaosudu<zuixiaotiaozhengshudu&&ylilunjiaosudu>weitiaoweizhi)ylilunjiaosudu=zuixiaotiaozhengshudu;9 r- x1 u) d: M9 r- n8 [+ g
if(zlilunjiaosudu<zuixiaotiaozhengshudu&&zlilunjiaosudu>weitiaoweizhi)zlilunjiaosudu=zuixiaotiaozhengshudu;
9 U- [7 ^; @  f4 ?  `if(xlilunjiaosudu>-zuixiaotiaozhengshudu&&xlilunjiaosudu<-weitiaoweizhi)xlilunjiaosudu=-zuixiaotiaozhengshudu;
& t, ~+ z5 d% O; _) C; x1 T. w/ dif(ylilunjiaosudu>-zuixiaotiaozhengshudu&&ylilunjiaosudu<-weitiaoweizhi)ylilunjiaosudu=-zuixiaotiaozhengshudu;$ Y" ~4 ^1 L  ^3 u; R1 u2 p
if(zlilunjiaosudu>-zuixiaotiaozhengshudu&&zlilunjiaosudu<-weitiaoweizhi)zlilunjiaosudu=-zuixiaotiaozhengshudu;
7 q. V, Q  Y) F}4 j7 }* r; u- p! L) ?7 |
{   //计算角速度平均数
) g, r9 e- h1 d) ~  P# _9 f# m# D9 }3 i+ v0 ]/ [
xjiaosudu=0;7 ]; T# d$ Q  `9 u2 H
yjiaosudu=0;
$ C- z+ F, _3 R3 Vzjiaosudu=0;" X) @2 x  y* j2 h
for(i=0;i<jiaosuduqudiancishu;i++)9 K- _: U" o5 V- J# {$ q
{) T7 r+ t" t0 O+ b
xjiaosudu+=xjsd[i];
8 }% d# X7 s  ^8 fyjiaosudu+=yjsd[i];2 A$ S* o- U) q1 d9 N2 z1 J" @
zjiaosudu+=zjsd[i];
8 S$ _# U* O+ n! E( E/ U' ~* x}
/ k( o- R- J' Y7 i; ?9 Bxjiaosudu/=jiaosuduqudiancishu;3 s& a' W0 f! P$ L! Z: b- R
yjiaosudu/=-jiaosuduqudiancishu;8 e3 Z+ |1 X* \. w& q
zjiaosudu=zjiaosudu/jiaosuduqudiancishu;
, g1 V/ h( _& D$ q- a}
( W, C0 L) X. ^" {6 F" j
3 O3 v9 S, c3 I" ]! j
7 K7 k3 z* A% Q: n/ I. Q" uif(shuruyoumen>1200) //如果油门处于激活状态
( Z" N7 a. |# W7 I/ l1 ]5 c9 p5 c{ //从理论角速度计算功电机运行的信号强度
! R9 y; J% O% a6 R5 `
5 n: u# J, o4 _% W! |' Cshijijiaosuduchangshu=shijijiaosuduchangshu2*(lilunyoumen-800)/1000*(lilunyoumen-600)/1000;+ |: m% T& k8 ~/ ]  Z5 I
lingshijiaosudu=(-xjiaosudu)/shijijiaosuduchangshu;# J$ S4 G) m' H
if(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;
0 Z7 ^/ H1 e8 f6 `" X) }6 r( oif(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;0 S& e" k# f' x* `3 |; I) b
//xjiaosudu=-shurufuyi/jiaosuduchangshu-lingshijiaosudu;//角速度调整模式时 jiaosuduchangshu=15
9 P) J/ x2 u2 ]  c//xlilunjiaosudu=-shurufuyi/jiaosuduchangshu;
- q$ c; u/ k- k) j9 B8 a* E* Sxjiaosudu=xlilunjiaosudu-lingshijiaosudu;  |' F* {+ {! D' `
lingshijiaosudu=(yjiaosudu)/shijijiaosuduchangshu;1 a5 j6 f# @& D" _1 u. _; |8 C8 e. l
if(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;1 j! S- M$ O/ h0 G" F* o0 W/ L+ [" G
if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;
5 [+ e, C& Y" F* G& {( {+ Y//yjiaosudu=shurushengjiang/jiaosuduchangshu-lingshijiaosudu;
) _9 s+ H% j1 _, |1 `. H//ylilunjiaosudu=shurushengjiang/jiaosuduchangshu;
5 m  H+ x1 g! o& Kyjiaosudu=ylilunjiaosudu-lingshijiaosudu;: r! d- }$ W8 F6 _( Y
lingshijiaosudu=(zjiaosudu)/shijijiaosuduchangshu;
6 q* f$ o# ?5 ?5 Fif(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;
$ r) {* w" k6 R  F' ~if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;6 f: S2 l$ U0 Z
//zjiaosudu=shurufangxiang/jiaosuduchangshu-lingshijiaosudu;; q1 I8 k; I) c0 O! Z3 s$ z
//zlilunjiaosudu=shurufangxiang/jiaosuduchangshu;
2 b: b$ q* H% `zjiaosudu=zlilunjiaosudu-lingshijiaosudu;
5 x6 m$ r* ?2 iif(lilunyoumen>1700)lilunyoumen=1700;
& ^2 O* ~2 F" w9 r  Qduojikongzhi[0]=lilunyoumen+xjiaosudu+yjiaosudu+zjiaosudu*3+duojipianyi[0];8 h5 E0 @+ s9 l  c& ~
duojikongzhi[1]=lilunyoumen-xjiaosudu+yjiaosudu-zjiaosudu*3+duojipianyi[1];
8 P- ]+ R% C; V6 [1 nduojikongzhi[2]=lilunyoumen-xjiaosudu-yjiaosudu+zjiaosudu*3+duojipianyi[2];
# ^8 H* c+ o2 J$ d' G3 pduojikongzhi[3]=lilunyoumen+xjiaosudu-yjiaosudu-zjiaosudu*3+duojipianyi[3];
5 u0 R1 s, ^# nfor(i=0;i<4;i++)8 ]9 ]* P% E1 C7 C& [# i( `# [/ Y
{- J: Z, r( X' s5 P. g
if(duojikongzhi[i]<1200)duojikongzhi[i]=1200;
# S4 V4 }' t+ x& v* W. g* R}8 L- h" D- ?6 P( b% f" N
}
- w. [( O9 i1 z# ]6 c. A9 I( {1 welse { //如果油门为0/ S" @) z* o5 [$ n  e/ x; E) @
duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=1100;& @. f( s/ B! d3 N, e
lilunfuyi=Roll;* I+ a* p$ r' \7 }4 Q! s. B& W
lilunshengjiang=Pitch;
0 ^8 z# h: p5 L# L# T9 Plilunfangxiang=Yaw;
! I6 V/ i4 [3 n6 V: e2 P1 p5 i5 c* A}* e/ c# H8 z5 [9 N- ~
//t=double(jieshouji[0])/100;
3 P3 y- m. T/ N& z% |/*$ i  b" M+ O9 W6 F+ A% b, y
for(i=0;i<6;i++){) D! U# f& H# |% s& a# c1 i' N; o. A2 i
}( R3 }1 Y: \: l3 D' {5 X
chuankoufasong(36,shurufuyi);/ {  j2 n, z! Z; j( {) X( z* u7 y
chuankoufasong(37,shurushengjiang);
% _$ P- Q: n" C7 ?( I! mchuankoufasong(38,shuruyoumen);& P' v9 f, [3 y. u6 ^
chuankoufasong(39,shurufangxiang);delay_ms(30);9 o) [; |  i6 x% X$ e: I
{ //发送串口数据,功电脑读取.
) g8 U$ ~1 k4 A2 Ih=shurufangxiang;" s7 K% l- D7 s: B3 O( m! o* u
chuankoufasong(36,xjiaosudu); delay_ms(10);1 w! F# `4 J  n1 Y6 w* V: t! W
h=shuruyoumen;. H1 G% }+ s! W3 o+ ~* x1 Z3 N
chuankoufasong(37,yjiaosudu); delay_ms(10);
* a7 p* |6 \9 u" y" w, w) Rh=chafangxiang*57;6 \7 ~, D7 g3 H" j
chuankoufasong(38,zjiaosudu);. n$ y3 }3 X/ S9 t6 T. N5 w8 T
delay_ms(10);
( x& i7 Z" s# B' w) Z+ i# G  Rchuankoufasong(39,duojikongzhi[3]);
5 W" a  [1 k9 X2 ]( Ydelay_ms(10);( M* v  W5 K; I2 O- o
//duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=shuruyoumen+1100;9 _3 z. d7 ^# s
}9 d* [8 C2 X+ G9 G( M2 L
h=xjiaosudu;) T' h# e  i$ u0 Y* i6 m- P
chuankoufasong(36,h);
0 a! q* \2 Q# o3 K5 I" n. Gh=yjiaosudu;
6 Z' R6 C4 B1 X  {& Ychuankoufasong(37,h); delay_ms(10);- x, E' p' q/ Y1 |% t$ q
h=zjiaosudu;
4 ~  p, S* M6 J1 s9 Y- f9 |chuankoufasong(38,h);
, W5 ?3 Q( n1 {5 fh=shurufangxiang;1 t. y" M2 x, \# U& n$ R; _
chuankoufasong(39,h);
7 a5 M4 }  P8 {5 uh=shuruyoumen;% W5 \! u2 G% P) S
chuankoufasong(39,jieshouji[4]);
6 k0 G- ?. T7 T! N& F% ~3 Gh= dumpu6050jicunqi(0);
# I; |8 l/ m2 R$ W9 zchuankoufasong(37,h);
, B. O' f8 b& d/ O( O8 v% ch= dumpu6050jicunqi(1);
7 U8 ^" I0 y% B+ kchuankoufasong(38,h);! Q& K6 ^$ r7 j
h= dumpu6050jicunqi(2);1 u8 l4 y  i; Q7 N3 ~! f5 W$ b
chuankoufasong(39,h);% T4 F4 d: I% y) q% n0 R
chuankoufasong(36,jieshouji[3]);3 c2 e) n7 B- m6 |  |/ B+ \, @& s
chuankoufasong(37,jieshouji[2]);
! N! ?7 z. U, kchuankoufasong(38,jieshouji[4]);
! V& U+ ~2 v# X( e1 ?7 G*/+ m& _2 N2 V! `, K# k5 U
{, \5 U0 ]% Z2 ]( x0 U. p  U) c
if((USART1->SR&0X40)!=0)
$ J4 q! S: L' u{8 h" t8 H* u+ J
* h$ n+ h* w9 F4 c: p" B. x- n  t
zhinengchuankou();  j  m% T+ G0 t3 z: ^8 S
}5 K- W4 f, P9 J$ n: [4 ?+ h
}
4 M3 j, D; ?- O9 s2 w4 e
$ O' I! H+ f5 ^}
" L1 F& `0 n" s
1 R5 o2 i- L4 g" g0 k- g$ W. S' ^}
% ?! C) ]+ x( Fvoid TIM2_IRQHandler(void) //定时器2 ,用于读取DMP数据. U5 e2 D* o0 T  f5 f
{ 0 J3 ]! E+ I' A( }2 f9 }
TIM2->SR&=~(1<<0);//清除中断标志位                  + r3 B4 p5 j3 s2 {
  
  o: |, |: m5 O$ z* e dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);  " L- ]. d) g% o" ~% k, b$ P
     if (sensors & INV_WXYZ_QUAT )
3 C+ r3 V- [6 p$ v6 E. C {
0 i4 I6 F7 \+ W2 Y- C5 X+ A   q0=quat[0] / q30;
, f5 L  K. v  A0 t0 {7 V q1=quat[1] / q30;. h0 i% d4 o9 ^& K: y
q2=quat[2] / q30;
. @* ]4 V) v6 w: \ q3=quat[3] / q30;
' f0 T9 E0 q( T$ C$ B5 t" {4 { Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); // roll副翼
9 f; V# G# n1 |& O Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2); // pitch升降5 `! k5 w) v' P/ H% r6 Y5 m8 |
   Yaw =  atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) ;//方向9 V' ^( j+ c% f2 X' ]& U
0 q+ J$ V0 p) _( b# G
//h=Roll*57295;- @, ~9 Q9 r" T8 k' H
//chuankoufasong(36,h);
# b6 }" n0 S, C' u* l. k //h=Pitch*57295;
2 w" n8 k. V% |4 d& m //chuankoufasong(37,h);- V$ |, v* O& V2 j) d
//h=Yaw*57295;
8 t: b- W: ~1 ]  ?* |// chuankoufasong(38,h);
; i5 o7 p9 O8 ]. O  P" _' K5 s+ G* t+ X0 n" D: X+ h6 N
// PrintChar("nihao\n");, b' Y0 c1 E, y4 x- f: j
}1 G' x' `: m, [0 s

1 B; k/ ?$ }7 j0 f. g$ f) zxjsd[jiaosuducishu]=(gyro[0]-0);0 I6 @: q2 \. ^+ @- Y
yjsd[jiaosuducishu]=-(gyro[1]-0);
7 @( A- K7 I! i6 {# N0 C# tzjsd[jiaosuducishu]=(gyro[2]-0);
4 d' N# x, L  f- _9 wjiaosuducishu++;1 w5 i: T7 {) j" ]
if(jiaosuducishu>=jiaosuduqudiancishu)jiaosuducishu=0;( d2 [3 q/ E1 ^

* o& y' f1 R; b0 `8 r, x0 ?}
3 l+ W* Q4 W0 H4 m4 f. t* ?  {% }- [% @! X

) j9 g! p' d! M7 d9 q. O
* w! b8 O, W, N$ @6 W
; b. c/ L8 U) r4 Y1 x+ }0 _0 m( N0 r4 w% _

0 X& u1 X, j  y! h6 x3 [
6 v; P2 k4 r3 y" F9 c
+ T* Q3 w5 k  F8 ?( p# `6 h, A1 [, I0 k& o+ ^. f
) x+ F  S4 m9 O4 m
2 _5 W  K4 f$ _; y# @* E; r/ u! s
void EXTI15_10_IRQHandler(void) //接收机信号接收中断         耗时2微秒  以内
: b( I: T2 j8 J. s{& J# ]# y5 d- n0 M9 @) g
u8 jicixunhuan;: O7 q8 g3 P: o2 d- K
for(jicixunhuan=12;jicixunhuan<16;jicixunhuan++)4 O& U$ n9 |- p
{
+ x) U3 I: e' [7 ~! Hif(((GPIOB->IDR>>jicixunhuan)&1)){  t8 P& `  _/ U0 M. t& o
shuruyinjiao=jicixunhuan;+ i' o8 T& k9 l( I0 S
TIM1->CNT=0;
% ?# s& E- b5 {- L* O! i, `}
4 c, r2 Q  Y% a5 W/ k}
9 h+ Z% t* _$ ^$ N6 X; Q$ j% ~
6 G5 Z9 S1 J- Z1 `/ L0 w9 [1 Lif(!(GPIOB->IDR>>12&0xf)){ //如果接收机发送结束
3 J. h  N4 R& `3 U. T( r$ Dif(TIM1->CNT>1000&&TIM1->CNT<2000)# P2 c# Z6 G# c  ?( S1 i) h9 y
{
* K; U0 B9 o# J% i* r: V" Z( Ljieshouji[shuruyinjiao-10]=TIM1->CNT; //将接收机变量保存到变量1 z  \# C* ^5 ~, G, L! E; A
jieshouji[shuruyinjiao-10]/=jieshoulingmin;
& `" H+ s% Y# m) i  Yjieshouji[shuruyinjiao-10]*=jieshoulingmin;
5 G1 ]& H/ T6 T) k}
0 S6 M+ B+ H+ G2 n, `if(shuruyinjiao==15)
& V# T4 x  s  g( i) L% w{* W8 i9 C3 ]! H0 I8 X
/////************************************************************************//
% r+ c2 p' C! ]; h. i( _5 {//计算得到每个摇杆的行程3 S; ?5 l; ?# L3 `$ c
if((youmen[0]-jieshouji[3])<100&&(shuruyoumen-jieshouji[3])<100)
- G- D, \8 X+ w% |# P0 H{
% V8 W  T3 ]2 V- d" s9 Tshurufuyi=1492-jieshouji[3];7 w# F/ z7 }0 R& p/ h9 U9 A
}
/ x& d# e2 k5 J6 |8 e- ryoumen[0]=jieshouji[3];
  m0 k8 o+ L9 k) w: o' |% \% D/ X- a+ wshurushengjiang=1492-jieshouji[2];
2 e( R) b% s5 p* S; O& vshurufangxiang=1492-jieshouji[5];# _! I0 [* }% D# o/ w
shuruyoumen=jieshouji[4];
! _: n0 A) u8 b) e' }5 r! a- {0 d1 G/////************************************************************************//* X1 @  n" T3 ?, t
//每个遥感的行程转化成每次的偏移量
" {. y+ S( m% ?' i  f6 gnifuyi=nifuyi+shurufuyi/kongzhilingmin;
1 z' O) v1 N# U# v/ Wnishengjiang=nishengjiang+shurushengjiang/kongzhilingmin;" X" _) y0 c& ~$ f/ E* [' N
nifangxiang=nifangxiang+shurufangxiang/kongzhilingmin;
" |. \$ e, C( w( V% ?+ w}
- J6 [) u- V5 {1 x8 \6 h( c}
4 }+ n' j2 x0 @) Rif(shuruyoumen<1190){jiesuo++;) s8 o- B# ]' D+ t
if(shurufangxiang>200){
# n2 {9 R6 _& ?& A9 zif(jiesuo>700) {
/ S2 x, |0 D. w+ @  F% c' rjiesuo=0;
; @! F+ V' R* j0 i# S7 b% v2 v- R* Ksuo=0;
% h2 W; p  @6 D5 n/ \# e}7 }& ^2 {' O% \' d4 T
}  t- i' _/ q4 G' o+ I" }( k
if(shurufangxiang<-200){
5 N6 n' P5 U; N, c1 c/ Tif(jiesuo>700){' h' i  }5 R3 P! v
jiesuo=0;8 G: Q6 N1 s1 {* I2 `/ s
suo=1;
7 k4 J% k3 t) w4 j+ {}" q0 k( p; j- u
}& P: }7 \  R/ f$ x
}
. g& M8 F% h3 Aelse jiesuo=0;2 E, v. ~1 S& L' R
zhongduancishu++;
- d; l5 ?' A, q" ]5 h8 IEXTI->PR=1<<10;
" ?, J; p; r; |. x! XEXTI->PR=1<<11;: [! R( X0 m& X* }& }
EXTI->PR=1<<12;# j2 a- e8 j/ M$ g
EXTI->PR=1<<13;
4 c# F& a% G4 m; E5 X3 PEXTI->PR=1<<14;3 d3 V+ J( t( k% K' O+ @
EXTI->PR=1<<15;" e8 J7 G6 [" k3 C% s1 F
}
2 c8 \; J) X/ p/ i$ E. w, W" w( y9 |% S1 t: G4 V2 I
void TIM3_IRQHandler(void) //定时器3,用于输出PWM信号   耗时1微秒 以内9 @% q/ r; U& H0 p
{ : r) Y: m0 s8 S! U  A
if(suo==1){
- Q- c3 p9 M0 j4 m" Kif(shuruyoumen<1200)duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=1100;
6 b$ Y0 }: D* [9 F5 q/ W- O2 TTIM3->CNT=duojikongzhi[wei];& Z1 `/ l5 D  {
if(wei==0){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji1=1;}- |' H; M: w# L
if(wei==1){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji2=1;}
2 O! U! U5 f) k6 q2 x- _if(wei==2){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji3=1;}4 x* \& U, m& p0 k- m
if(wei==3){duoji1=0;duoji2=0;duoji3=0;duoji4=0;duoji4=1;led4=!led4;}. b# q; A- b. Y9 X+ h
if(wei==4){duoji1=0;duoji2=0;duoji3=0;duoji4=0; }2 ~! x, z. h# W6 }' [5 h% ^
               
$ o( w6 B8 G0 K& S+ S& o5 n  ^; L4 ^0 }8 R% @! f
wei++;
# C2 d! O. }+ L$ kif(wei>4)wei=0;  a& K5 {' `9 k8 X* S3 x8 G; I
}1 _1 d4 w  T5 Y: _+ Y: g" ~, B
if(suo==0)led4=0;/ G8 n  e. i# k5 R7 v4 ~/ K
TIM3->SR&=~(1<<0);//清除中断标志位  
, Y; q$ c, Q* K+ D# _4 Q$ h}$ g0 \* w) j9 F7 Y

% }2 A" y/ U2 W- r! Z! B8 h! S0 I9 zvoid TIM4_IRQHandler(void)
7 x! H: E% O1 d# w' w3 s$ V{
6 E/ x! `) J. ~2 I4 t  HTIM4->SR&=~(1<<0);//清除中断标志?* K/ U. ?: c3 K' d! t# H- W
/*? , v. g+ C9 `+ f* F7 x( y

+ y: d7 j, q; S; E+ c*/7 W4 f; M; h( t% \2 o
}
, e2 j: x9 C- l6 N$ ^  [
& k5 r' P# B7 ]+ i+ F( P
4 \/ h# Y# d# n2 H# ?2 c- p3 uzhinengchuankou()
0 i  x6 M" s% i4 t9 s! ~# I- J" _{! g5 L  T+ H7 `. w
USART1->SR&=~(1<<6);
/ O3 k. n6 v, K0 W- R. wchuankoufasongwei++;' P9 q* }- w$ L! n! @! N
if(chuankoufasongwei>=20){chuankoufasongge=1;) K/ F2 K  S% X- f1 U
if(chuankoufasongcishu==36){
" j5 s8 H& N& ?5 ^. }SendBuff[0]=36;8 v2 m( l1 a: X8 {( K- u( N1 U
chuankoufasongshuju=Roll*572957;
2 b- ~1 l9 @3 C! @5 O5 echuankoufasongcishu=37;
4 f) g$ M$ o5 M0 f5 r0 S" k}else
7 n6 e8 R% }7 N7 C' C& sif(chuankoufasongcishu==37){
" U! i3 [% W* H$ T! A- nSendBuff[0]=37;
1 G$ c$ x5 T. B6 J( k8 d& ochuankoufasongshuju=Pitch*572957;
) T5 `/ p/ g  Q" w5 I# ]" _chuankoufasongcishu=38;
% k7 h, P: s3 N}else
6 K, v, k5 [/ X* v3 R- ~, G3 Pif(chuankoufasongcishu==38){; R/ E( U0 m) k9 B
SendBuff[0]=38;/ f! H3 M$ \. e0 J9 X
chuankoufasongshuju=Yaw*572957;: G) Q7 w5 |4 Y$ `' A" t
chuankoufasongcishu=36;: F8 Z0 A# f) W/ j8 {8 e  ~( x* K
}
. ^( y8 G1 ]* x3 }1 a0 ]if(chuankoufasongshuju<0){
! c. D) [3 X' S+ @: iSendBuff[chuankoufasongge]=45;  // 给SBUFA数据
- K, n- u4 ^/ e6 B" Fchuankoufasongge++;
  X+ T# i; ^5 |3 I}- t7 z9 v1 Q; T! n- }

6 s8 z' c9 Q# A& ]* qif(chuankoufasongshuju>=100000000){
( j& u% _3 F4 x& B% O' v( T. N# b) X- ^SendBuff[chuankoufasongge]=(chuankoufasongshuju/100000000)%10+48;  // 给SBUFA数据
/ e. L& ?% b9 b7 |chuankoufasongge++;) _2 x5 J0 z7 u! R. A0 v6 Z
}& s- W, C$ J1 N; Q. u) C
if(chuankoufasongshuju>=10000000){
; e" s$ F4 y; o& SSendBuff[chuankoufasongge]=(chuankoufasongshuju/10000000)%10+48;  // 给SBUFA数据
6 C  K+ ]1 L* T% C) V- ~7 ichuankoufasongge++;
, n4 E( Y% C! G" a" X: X& L}
/ s7 ], j- s- T! g' N6 h% C5 Yif(chuankoufasongshuju>=1000000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1000000)%10+48;  // 给SBUFA数据9 M" S5 P; w* T: k$ P4 W
chuankoufasongge++;3 v9 r. ~, [) s# ]! x2 i
}8 M" Z6 ?% d0 i5 y
if(chuankoufasongshuju>=100000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/100000)%10+48;  // 给SBUFA数据6 f, z8 f/ E* {1 E: C# @8 L2 [
chuankoufasongge++;
9 @- e* ?* x( E$ B% p9 e}
& @# j* e2 Y! s  d( wif(chuankoufasongshuju>=10000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10000)%10+48;  // 给SBUFA数据; z" t4 B3 k( C* t
chuankoufasongge++;
0 `( t8 ^+ ]+ Z( U" I4 R4 p}
2 I" ]) P1 s& Dif(chuankoufasongshuju>=1000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1000)%10+48;  // 给SBUFA数据: p' e% T% ]) V5 f" `8 D' l' E
chuankoufasongge++;
& D/ F3 E0 I) W! N5 |/ f}
+ ]2 S* d6 q5 y4 f6 T5 B% p$ tif(chuankoufasongshuju>=100) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/100)%10+48;  // 给SBUFA数据7 [& v9 W9 p! Y0 K. X
chuankoufasongge++;
5 S6 x6 z1 j9 X8 u  ?4 K3 P- [( ?}0 S" T+ @, n, n: f0 d
if(chuankoufasongshuju>=10) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10)%10+48;  // 给SBUFA数据
/ o9 X/ P& B% G- M7 {8 lchuankoufasongge++;
) i& N1 l  O3 Z  w# J/ K2 I}
) O; Z* b5 a. }; |/ {if(1) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1)%10+48;  // 给SBUFA数据
8 X0 e# q7 w  o. F6 z( schuankoufasongge++;! u, u, k- b' i7 ?
}
! d* O2 V" F- ^/ tfor(;chuankoufasongge<20;chuankoufasongge++)
. \2 T( v1 Z/ t/ g: ^; g  WSendBuff[chuankoufasongge]=10;9 L  w% `' p5 R- Q
chuankoufasongwei=0;
9 j: r; {* d$ E/ ?}
& K( n/ c5 b& Z& GUSART1->DR=SendBuff[chuankoufasongwei];
9 `( U" ]# ?. B. Q! L/ l}. d8 {! ~/ z% \1 P& R* j6 s8 g
foxglove 回答时间:2015-3-13 14:59:33
这些代码仔细看看不错
wsnfly~ 回答时间:2015-3-13 15:55:48
foxglove 发表于 2015-3-13 14:59
) h4 x8 Y8 O" b! Y) J; y9 y6 u/ s! o这些代码仔细看看不错

2 y& Y9 e( i3 d3 g2 l& r我写的乱七八糟的,很长时间没看,自己都看的累

所属标签

相似技术帖

官网相关资源

关于
我们是谁
投资者关系
意法半导体可持续发展举措
创新与技术
意法半导体官网
联系我们
联系ST分支机构
寻找销售人员和分销渠道
社区
媒体中心
活动与培训
隐私策略
隐私策略
Cookies管理
行使您的权利
官方最新发布
人形机器人运动控制、感知与智能配电
半导体创新技术与应用方向
EE架构与软件定义汽车
12V/48V 汽车智能配电(SPD)
区域控制单元(ZCU)与分区架构
关注我们
st-img 微信公众号
st-img 手机版