|
飞行控制算法还不是很好,今年有时间继续完善一下,先上视频看看,大家有兴趣的一起学习 好像不能直接放视频哦,那只能跳转到优酷了 一个是制作调试的视频$ 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 ( b1 h4 F4 X0 t3 U% I! x3 o 这个是飞机上FPV的视频 https://v.youku.com/v_show/id_XNzI0NDkxNTI4.html! n, f% `+ R$ c- ]) M |
DMA 原理从 0 到 1:为什么它能解放 CPU?一文讲透 DMA 的工作流程
【福利三:雨露均沾·逢7狂欢】之五:STM32系统时钟PLL故障排除1例
【福利三:雨露均沾·逢7狂欢】之三:AI帮我查BUG
福利三:雨露均沾·逢7狂欢】之二:STM32F407VG串口通信乱码故障的排除
基于STM32F103的I2C主从机通信
OpenBLT移植到STM32F103战舰开发板上适用于所有STM32F103系列的Bootloader
TFT LCD 与 FSMC 的硬件连接大容量 STM32F10xxx FSMC 接口
单片机:初学者该了解的STM32F103基础知识
2025软件工具兔哥知道
STM32之继电器模块
微信公众号
手机版
当然可以,就是写的太乱了怕大家看不清楚. v6 [* q" @/ ]0 I
#include "usart.h"
#include "delay.h"
#include "led.h"
#include "key.h"
#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"
#include "rtc.h"
#include "wkup.h"
#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"
#include "flash.h"
#include "24l01.h"# I- b& A+ a1 Z; E
#include "mmc_sd.h"
#include "remote.h"! J" g5 U2 N; W+ g' t
#include "ds18b20.h"
#include "mouse.h"
#include "text.h"
#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
zhinengchuankou();
#define duoji1 PAout(5)// PA8
#define duoji2 PAout(1)
#define duoji3 PAout(0)* a. B) g3 T6 W I7 k+ g
#define duoji4 PAout(2)5 }% ?" D# x" j; M$ O4 P
s16 duojipianyi[4];
//u16 jiaosuduchangshu=50,jieshoulingmin=4,shijijiaosuduchangshu=50,zuidaxiuzhengsudu=60,zuixiaotiaozhengshudu=10,weitiaoweizhi=10,jiaosuduqudiancishu=5,zuidatiaozhengsudu=40;//可用
( 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;
# 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;
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];+ s& {- |% N: K8 j
double nifuyi=0,nishengjiang=0,nifangxiang=0,niyoumen=0;
double 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};
8 f+ R' k( q6 S$ i# A6 I# v
: g: I o5 H1 }% M3 L+ F+ v
int main(void)
{$ l& w6 D% i" ^$ h5 g: d
// u16 i;
{
duojipianyi[0]=0;0 C( o) W( G8 T. a
duojipianyi[1]=0;
duojipianyi[2]=0;
duojipianyi[3]=0;
duojikongzhi[0]=1100;
duojikongzhi[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;
duojikongzhi[4]=2000;
}/ L& N" @1 g7 Y, k7 H& b
Stm32_Clock_Init(9); //系统时钟设置
delay_init(72); //延时初始化
uart_init(72,1600); //串口1初始化 ' v$ Q4 {* y9 k( w* n' h- |" D* ^2 }8 M
//USART1->DR=SendBuff[1];
gpioshezhi(); //引脚设置1 E4 H) @* ^) ?, l5 G3 ^+ M& D
led4=0;
// LED_Init(); //LED初始化9 J( ]% A) o/ e+ \% x$ m: A( ~
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;//定时器一,没有中断,用于计时接收机信号- 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
chuankoufasong(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制造误差
xpianyijiaosudu=dumpu6050jicunqi(0);
ypianyijiaosudu=dumpu6050jicunqi(1);
zpianyijiaosudu=dumpu6050jicunqi(2);2 c# y' R( U% T5 l+ J
}
if(!result)0 |$ G5 _, J& x9 F: J+ v
{
chuankoufasong(36,2);
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);
chuankoufasong(37,4);+ x5 Y M, a( I+ @4 h
USART1->DR=SendBuff[chuankoufasongwei];
while(1) ! {$ J8 M" \: x
{# F; ?: K2 { M3 x
{ //计算出理论飞行角度% N8 s F C7 G" A" k3 T0 j1 v* m
/////************************************************************************//
lilunyoumen=shuruyoumen;
lilunfuyi=0.4*(shurufuyi/300);
lilunshengjiang=0.4*(shurushengjiang/300);
lilunfangxiang=lilunfangxiang+nifangxiang*2 ;7 A, b. D) E c4 |- A1 `, }* @! g! v
nifuyi=0;: b0 O( |0 e8 Y) z: `9 R
nishengjiang=0;
nifangxiang=0;
if(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;}
if(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;
if(lilunfuyi>3.14159)lilunfuyi=lilunfuyi-3.14159*2;
if(lilunfuyi<-3.14159)lilunfuyi=lilunfuyi+3.14159*2;
}: ^( d: W& U8 Q6 Y, D! ^1 V
{ //计算出实际坐标和理论坐标差
chafuyi=Roll-lilunfuyi;3 o1 Q" o8 z9 n7 `* `
if(chafuyi>pi)chafuyi-=2*pi;
if(chafuyi<-pi)chafuyi+=2*pi;# `( E' o' y) ^: R( T
//if(chafuyi>(pi/8))chafuyi=pi/8;
//if(chafuyi<(-pi)/8)chafuyi=-pi/8;3 `. `* D1 ~) ?2 i; D
chashengjiang=-(Pitch-lilunshengjiang);
if(chashengjiang>(pi))chashengjiang-=2*pi;
if(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;
chafangxiang=-(Yaw-lilunfangxiang);
if(chafangxiang>pi)chafangxiang-=2*pi;- H3 m; w: K" c( L0 o
if(chafangxiang<-pi)chafangxiang+=2*pi;
//if(chafangxiang>(pi/8))chafangxiang=pi/8;8 y. ?) p e6 G8 r
//if(chafangxiang<(-pi)/8)chafangxiang=-pi/8;
}
{ //计算出飞行器各个轴需要的理论角速度
xlilunjiaosudu=(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;
if(xlilunjiaosudu>zuidatiaozhengsudu)xlilunjiaosudu=zuidatiaozhengsudu;: c( F. C1 h2 o( n: B
if(xlilunjiaosudu<-zuidatiaozhengsudu)xlilunjiaosudu=-zuidatiaozhengsudu;
if(ylilunjiaosudu>zuidatiaozhengsudu)ylilunjiaosudu=zuidatiaozhengsudu;7 z) I- L# K6 _- a6 i
if(ylilunjiaosudu<-zuidatiaozhengsudu)ylilunjiaosudu=-zuidatiaozhengsudu;
if(zlilunjiaosudu>zuidatiaozhengsudu)zlilunjiaosudu=zuidatiaozhengsudu;
if(zlilunjiaosudu<-zuidatiaozhengsudu)zlilunjiaosudu=-zuidatiaozhengsudu;" Q8 {$ z" A' r1 v: i
if(xlilunjiaosudu<zuixiaotiaozhengshudu&&xlilunjiaosudu>weitiaoweizhi)xlilunjiaosudu=zuixiaotiaozhengshudu;
if(ylilunjiaosudu<zuixiaotiaozhengshudu&&ylilunjiaosudu>weitiaoweizhi)ylilunjiaosudu=zuixiaotiaozhengshudu;9 r- x1 u) d: M9 r- n8 [+ g
if(zlilunjiaosudu<zuixiaotiaozhengshudu&&zlilunjiaosudu>weitiaoweizhi)zlilunjiaosudu=zuixiaotiaozhengshudu;
if(xlilunjiaosudu>-zuixiaotiaozhengshudu&&xlilunjiaosudu<-weitiaoweizhi)xlilunjiaosudu=-zuixiaotiaozhengshudu;
if(ylilunjiaosudu>-zuixiaotiaozhengshudu&&ylilunjiaosudu<-weitiaoweizhi)ylilunjiaosudu=-zuixiaotiaozhengshudu;$ Y" ~4 ^1 L ^3 u; R1 u2 p
if(zlilunjiaosudu>-zuixiaotiaozhengshudu&&zlilunjiaosudu<-weitiaoweizhi)zlilunjiaosudu=-zuixiaotiaozhengshudu;
}4 j7 }* r; u- p! L) ?7 |
{ //计算角速度平均数
9 f# m# D9 }3 i+ v0 ]/ [
xjiaosudu=0;7 ]; T# d$ Q `9 u2 H
yjiaosudu=0;
zjiaosudu=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];
yjiaosudu+=yjsd[i];2 A$ S* o- U) q1 d9 N2 z1 J" @
zjiaosudu+=zjsd[i];
}
xjiaosudu/=jiaosuduqudiancishu;3 s& a' W0 f! P$ L! Z: b- R
yjiaosudu/=-jiaosuduqudiancishu;8 e3 Z+ |1 X* \. w& q
zjiaosudu=zjiaosudu/jiaosuduqudiancishu;
}
if(shuruyoumen>1200) //如果油门处于激活状态
{ //从理论角速度计算功电机运行的信号强度
shijijiaosuduchangshu=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;
if(lingshijiaosudu<-zuidaxiuzhengsudu)lingshijiaosudu=-zuidaxiuzhengsudu;0 S& e" k# f' x* `3 |; I) b
//xjiaosudu=-shurufuyi/jiaosuduchangshu-lingshijiaosudu;//角速度调整模式时 jiaosuduchangshu=15
//xlilunjiaosudu=-shurufuyi/jiaosuduchangshu;
xjiaosudu=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;
//yjiaosudu=shurushengjiang/jiaosuduchangshu-lingshijiaosudu;
//ylilunjiaosudu=shurushengjiang/jiaosuduchangshu;
yjiaosudu=ylilunjiaosudu-lingshijiaosudu;: r! d- }$ W8 F6 _( Y
lingshijiaosudu=(zjiaosudu)/shijijiaosuduchangshu;
if(lingshijiaosudu>zuidaxiuzhengsudu)lingshijiaosudu=zuidaxiuzhengsudu;
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;
zjiaosudu=zlilunjiaosudu-lingshijiaosudu;
if(lilunyoumen>1700)lilunyoumen=1700;
duojikongzhi[0]=lilunyoumen+xjiaosudu+yjiaosudu+zjiaosudu*3+duojipianyi[0];8 h5 E0 @+ s9 l c& ~
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++)8 ]9 ]* P% E1 C7 C& [# i( `# [/ Y
{- J: Z, r( X' s5 P. g
if(duojikongzhi[i]<1200)duojikongzhi[i]=1200;
}8 L- h" D- ?6 P( b% f" N
}
else { //如果油门为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;
lilunfangxiang=Yaw;
}* e/ c# H8 z5 [9 N- ~
//t=double(jieshouji[0])/100;
/*$ 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);
chuankoufasong(38,shuruyoumen);& P' v9 f, [3 y. u6 ^
chuankoufasong(39,shurufangxiang);delay_ms(30);9 o) [; | i6 x% X$ e: I
{ //发送串口数据,功电脑读取.
h=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);
h=chafangxiang*57;6 \7 ~, D7 g3 H" j
chuankoufasong(38,zjiaosudu);. n$ y3 }3 X/ S9 t6 T. N5 w8 T
delay_ms(10);
chuankoufasong(39,duojikongzhi[3]);
delay_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);
h=yjiaosudu;
chuankoufasong(37,h); delay_ms(10);- x, E' p' q/ Y1 |% t$ q
h=zjiaosudu;
chuankoufasong(38,h);
h=shurufangxiang;1 t. y" M2 x, \# U& n$ R; _
chuankoufasong(39,h);
h=shuruyoumen;% W5 \! u2 G% P) S
chuankoufasong(39,jieshouji[4]);
h= dumpu6050jicunqi(0);
chuankoufasong(37,h);
h= dumpu6050jicunqi(1);
chuankoufasong(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]);
chuankoufasong(38,jieshouji[4]);
*/+ m& _2 N2 V! `, K# k5 U
{, \5 U0 ]% Z2 ]( x0 U. p U) c
if((USART1->SR&0X40)!=0)
{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
}
}
}
void 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 {
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more); " L- ]. d) g% o" ~% k, b$ P
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;. h0 i% d4 o9 ^& K: y
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升降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);
//h=Pitch*57295;
//chuankoufasong(37,h);- V$ |, v* O& V2 j) d
//h=Yaw*57295;
// chuankoufasong(38,h);
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
xjsd[jiaosuducishu]=(gyro[0]-0);0 I6 @: q2 \. ^+ @- Y
yjsd[jiaosuducishu]=-(gyro[1]-0);
zjsd[jiaosuducishu]=(gyro[2]-0);
jiaosuducishu++;1 w5 i: T7 {) j" ]
if(jiaosuducishu>=jiaosuduqudiancishu)jiaosuducishu=0;( d2 [3 q/ E1 ^
}
* ? {% }- [% @! X
+ }0 _0 m( N0 r4 w% _
, 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微秒 以内
{& 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
{
if(((GPIOB->IDR>>jicixunhuan)&1)){ t8 P& ` _/ U0 M. t& o
shuruyinjiao=jicixunhuan;+ i' o8 T& k9 l( I0 S
TIM1->CNT=0;
}
}
if(!(GPIOB->IDR>>12&0xf)){ //如果接收机发送结束
if(TIM1->CNT>1000&&TIM1->CNT<2000)# P2 c# Z6 G# c ?( S1 i) h9 y
{
jieshouji[shuruyinjiao-10]=TIM1->CNT; //将接收机变量保存到变量1 z \# C* ^5 ~, G, L! E; A
jieshouji[shuruyinjiao-10]/=jieshoulingmin;
jieshouji[shuruyinjiao-10]*=jieshoulingmin;
}
if(shuruyinjiao==15)
{* W8 i9 C3 ]! H0 I8 X
/////************************************************************************//
//计算得到每个摇杆的行程3 S; ?5 l; ?# L3 `$ c
if((youmen[0]-jieshouji[3])<100&&(shuruyoumen-jieshouji[3])<100)
{
shurufuyi=1492-jieshouji[3];7 w# F/ z7 }0 R& p/ h9 U9 A
}
youmen[0]=jieshouji[3];
shurushengjiang=1492-jieshouji[2];
shurufangxiang=1492-jieshouji[5];# _! I0 [* }% D# o/ w
shuruyoumen=jieshouji[4];
/////************************************************************************//* X1 @ n" T3 ?, t
//每个遥感的行程转化成每次的偏移量
nifuyi=nifuyi+shurufuyi/kongzhilingmin;
nishengjiang=nishengjiang+shurushengjiang/kongzhilingmin;" X" _) y0 c& ~$ f/ E* [' N
nifangxiang=nifangxiang+shurufangxiang/kongzhilingmin;
}
}
if(shuruyoumen<1190){jiesuo++;) s8 o- B# ]' D+ t
if(shurufangxiang>200){
if(jiesuo>700) {
jiesuo=0;
suo=0;
}7 }& ^2 {' O% \' d4 T
} t- i' _/ q4 G' o+ I" }( k
if(shurufangxiang<-200){
if(jiesuo>700){' h' i }5 R3 P! v
jiesuo=0;8 G: Q6 N1 s1 {* I2 `/ s
suo=1;
}" q0 k( p; j- u
}& P: }7 \ R/ f$ x
}
else jiesuo=0;2 E, v. ~1 S& L' R
zhongduancishu++;
EXTI->PR=1<<10;
EXTI->PR=1<<11;: [! R( X0 m& X* }& }
EXTI->PR=1<<12;# j2 a- e8 j/ M$ g
EXTI->PR=1<<13;
EXTI->PR=1<<14;3 d3 V+ J( t( k% K' O+ @
EXTI->PR=1<<15;" e8 J7 G6 [" k3 C% s1 F
}
. 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){
if(shuruyoumen<1200)duojikongzhi[0]=duojikongzhi[1]=duojikongzhi[2]=duojikongzhi[3]=1100;
TIM3->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;}
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% ^
& o5 n ^; L4 ^0 }8 R% @! f
wei++;
if(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);//清除中断标志位
}$ g0 \* w) j9 F7 Y
void TIM4_IRQHandler(void)
{
TIM4->SR&=~(1<<0);//清除中断标志?* K/ U. ?: c3 K' d! t# H- W
/*? , v. g+ C9 `+ f* F7 x( y
*/7 W4 f; M; h( t% \2 o
}
zhinengchuankou()
{! g5 L T+ H7 `. w
USART1->SR&=~(1<<6);
chuankoufasongwei++;' P9 q* }- w$ L! n! @! N
if(chuankoufasongwei>=20){chuankoufasongge=1;) K/ F2 K S% X- f1 U
if(chuankoufasongcishu==36){
SendBuff[0]=36;8 v2 m( l1 a: X8 {( K- u( N1 U
chuankoufasongshuju=Roll*572957;
chuankoufasongcishu=37;
}else
if(chuankoufasongcishu==37){
SendBuff[0]=37;
chuankoufasongshuju=Pitch*572957;
chuankoufasongcishu=38;
}else
if(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
}
if(chuankoufasongshuju<0){
SendBuff[chuankoufasongge]=45; // 给SBUFA数据
chuankoufasongge++;
}- t7 z9 v1 Q; T! n- }
if(chuankoufasongshuju>=100000000){
SendBuff[chuankoufasongge]=(chuankoufasongshuju/100000000)%10+48; // 给SBUFA数据
chuankoufasongge++;) _2 x5 J0 z7 u! R. A0 v6 Z
}& s- W, C$ J1 N; Q. u) C
if(chuankoufasongshuju>=10000000){
SendBuff[chuankoufasongge]=(chuankoufasongshuju/10000000)%10+48; // 给SBUFA数据
chuankoufasongge++;
}
if(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++;
}
if(chuankoufasongshuju>=10000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10000)%10+48; // 给SBUFA数据; z" t4 B3 k( C* t
chuankoufasongge++;
}
if(chuankoufasongshuju>=1000) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1000)%10+48; // 给SBUFA数据: p' e% T% ]) V5 f" `8 D' l' E
chuankoufasongge++;
}
if(chuankoufasongshuju>=100) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/100)%10+48; // 给SBUFA数据7 [& v9 W9 p! Y0 K. X
chuankoufasongge++;
}0 S" T+ @, n, n: f0 d
if(chuankoufasongshuju>=10) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/10)%10+48; // 给SBUFA数据
chuankoufasongge++;
}
if(1) {SendBuff[chuankoufasongge]=(chuankoufasongshuju/1)%10+48; // 给SBUFA数据
chuankoufasongge++;! u, u, k- b' i7 ?
}
for(;chuankoufasongge<20;chuankoufasongge++)
SendBuff[chuankoufasongge]=10;9 L w% `' p5 R- Q
chuankoufasongwei=0;
}
USART1->DR=SendBuff[chuankoufasongwei];
}. d8 {! ~/ z% \1 P& R* j6 s8 g
我写的乱七八糟的,很长时间没看,自己都看的累