基于STM32平台的Mini四轴飞行器设计

基于STM32平台的Mini四轴飞行器设计摘要：迷你四轴飞行器是一种更为精便好用的一款无人机，在时代发展以来，正在以快速的姿态进入人们的视野，大街小巷，草原沙滩，经常可以看见他的身影。本文是自己设计的迷你四轴飞行器，以2.4G无线数据传输作为飞行器的控制方式。通过MPU6050传感器对当前姿态进行解算，对比目标姿态，以PID算法来控制调整飞行器的电机转速和飞行姿态，使其能任以控制。关键词：无人机；迷你四轴飞行器；stm32；姿态解算

目录TOC\o"1-3"\h\u7279第1章引言 参考文献[1]夏渊湛,张林成,陈佳俊,李稳国.基于STM32的语音控制四轴飞行器设计[J].信息与电脑(理论版),2022,34(10):180-182.[2]吴春玉.农用四轴飞行器避障控制系统设计——基于机器视觉和超声波测距[J].农机化研究,2022,44(04):110-114.DOI:10.13427/ki.njyi.2022.04.019.[3]闫晓兵,陈春梅,白雪艳,刘鹏.基于STM32F4处理器的四轴飞行器控制系统设计[J].现代信息科技,2021,5(05):52-54+59.DOI:10.19850/ki.2096-4706.2021.05.012.[4]陈侠宇.四轴飞行器机身创成式设计及其SLM成形工艺研究[D].福建工程学院,2021.DOI:10.27865/ki.gfgxy.2021.000057.[5]宋权霖,张伟,刘业钊,方翔.四轴飞行器系统设计与制作[J].电子制作,2020(07):20-22.DOI:10.16589/11-3571/tn.2020.07.007.[6]张佳奇.四轴飞行器跟踪与控制系统设计[D].哈尔滨理工大学,2020.DOI:10.27063/ki.ghlgu.2020.000841.[7]田东波,徐鹏程,张德俊,周琦,杨云露.基于树莓派的四轴飞行器设计[J].智能计算机与应用,2020,10(03):279-283+287.[8]王鑫.基于四轴飞行器的高压线巡检系统设计与实现[J].电子制作,2020(01):10-11.DOI:10.16589/11-3571/tn.2020.01.004.[9]刘宝媛,龚赛君,崔治.基于STM32的四轴飞行器设计与实现[J].电子测试,2019(19):22-23+37.DOI:10.16520/ki.1000-8519.2019.19.006.[10]吴振磊,王轩,杨辉.基于Arduino平台的四轴飞行器设计[J].甘肃科技纵横,2019,48(07):30-32.[11]于明军,贾晓艳.面向中学生创客教育的简易四轴飞行器设计[J].赤峰学院学报(自然科学版),2019,35(07):91-94.DOI:10.13398/ki.issn1673-260x.2019.07.027.[12]付超.四轴飞行器控制系统设计与组合导航研究[D].天津大学,2018.DOI:10.27356/ki.gtjdu.2018.002333.[13]王鑫.一种新型的电力高压电网系统巡检四轴飞行器设计[J].内燃机与配件,2018(12):22-23.DOI:10.19475/ki.issn1674-957x.2018.12.011.[14]李强.四轴飞行器飞控系统的设计和研究[D].南昌大学,2018.[15]潘翔.一种航拍四轴飞行器控制系统设计[D].上海应用技术大学,2018.[16]蒋宏.面向隧道巡检的四轴飞行器平台设计与实现[D].电子科技大学,2018.[17]赵铭.用于光伏阵列巡检的四轴飞行器系统设计[D].合肥工业大学,2018.[18]陈俊如.基于STM32的四轴飞行器研究与设计[D].华东师范大学,2018.[19]赵兴章.四轴飞行器设计[J].散文百家(新语文活页),2018(02):244.[20]王瑞,刘莉苹.基于STC15的四轴飞行器设计[J].现代制造技术与装备,2017(12):85-86.DOI:10.16107/ki.mmte.2017.0988.附录相关代码及飞行器相关图库A1Keil工程遥控部分代码#include"Maths.h"#include"Control.h"#include"struct_all.h"uint8_tRc_Lock=1;//1ÉÏËø£»0½âËø/******************************************************************************º¯ÊýÔ­ÐÍ£º voidRC_Limit(struct_Rc*rc)¹¦ÄÜ£º ÏÞÖÆÒ£¿ØÖ¸ÁΧ*******************************************************************************/voidRC_Limit(struct_Rc*rc){ rc->THROTTLE=(rc->THROTTLE<=1000)?1000:rc->THROTTLE; rc->THROTTLE=(rc->THROTTLE>=2000)?2000:rc->THROTTLE; rc->PITCH=(rc->PITCH<=1000)?1000:rc->PITCH; rc->PITCH=(rc->PITCH>=2000)?2000:rc->PITCH; rc->ROLL=(rc->ROLL<=1000)?1000:rc->ROLL; rc->ROLL=(rc->ROLL>=2000)?2000:rc->ROLL; rc->YAW=(rc->YAW<=1000)?1000:rc->YAW; rc->YAW=(rc->YAW>=2000)?2000:rc->YAW; rc->AUX1=(rc->AUX1<=1000)?1000:rc->AUX1; rc->AUX1=(rc->AUX1>=2000)?2000:rc->AUX1; rc->AUX2=(rc->AUX2<=1000)?1000:rc->AUX2; rc->AUX2=(rc->AUX2>=2000)?2000:rc->AUX2; rc->AUX3=(rc->AUX3<=1000)?1000:rc->AUX3; rc->AUX3=(rc->AUX3>=2000)?2000:rc->AUX3;} /******************************************************************************º¯ÊýÔ­ÐÍ£º voidRC_LOCK(void)¹¦ÄÜ£º Ò£¿ØÊÖÊÆÖ¸Áî¼°½âËø´¦Àí£¨ÒÔÏÂ×¢ÊÍÕë¶ÔÃÀ¹úÊÖ£¬Ò£¿ØĬÈϵľÍÊÇÃÀ¹úÊÖ£©*******************************************************************************/voidRC_LOCK(void){ staticuint8_tcount0,count1,count2; if(Rc.THROTTLE<1300)//&&Rc.YAW>1700&&Rc.PITCH>1400&&Rc.PITCH<1600) count0++; else count0=0; if(count0>15&&Rc_Lock==1) { Rc_Lock=0;//×óÊÖÓÍÃÅÊÖµÄÒ¡¸Ë´òÏòÓÒϽǣ¬ÓÒÊÖÒ¡¸Ë²»¶¯£¬½âËø LED3_OFF; Delay_led(100); LED3_ON; Delay_led(100); }//////////////////////////////////////////////// if(Rc.THROTTLE<1300&&Rc.YAW<1300&&Rc.PITCH>1400&&Rc.PITCH<1600) count1++; else count1=0; if(count1>150&&Rc_Lock==0) { Rc_Lock=1;//×óÊÖÓÍÃÅÊÖµÄÒ¡¸Ë´òÏò×óϽǣ¬ÓÒÊÖÒ¡¸Ë²»¶¯£¬ÉÏËø LED3_OFF; Delay_led(100); LED3_ON; Delay_led(100); }//////////////////////////////////////////////// if(Rc.THROTTLE<1300&&Rc.YAW<1300&&Rc.PITCH<1300) count2++; else count2=0; if(count2>100)//&&Rc_Lock)//ÉÏËø״̬²ÅÄÜУÕý { count2=0; Do_GYRO_Offset();//×óÊÖÓÍÃÅÊÖµÄÒ¡¸Ë´òÏò×óϽǣ¬ÓÒÊÖÒ¡¸Ë¸©Ñö·½ÏòÀ­µ½×îµ×£¬Ð£ÕýÍÓÂÝÒÇ Do_ACC_Offset();//×óÊÖÓÍÃÅÊÖµÄÒ¡¸Ë´òÏò×óϽǣ¬ÓÒÊÖÒ¡¸Ë¸©Ñö·½ÏòÀ­µ½×îµ×£¬Ð£Õý¼ÓËÙ¶È¼Æ LED3_OFF; Delay_led(50); LED3_ON; Delay_led(50); LED3_OFF; Delay_led(50); LED3_ON; }}#defineangle_max 10.0f #defineangle_integral_max 1000.0f /******************************************************************************º¯ÊýÔ­ÐÍ£º voidControl_Angle(struct_out_angle*angle,struct_Rc*rc)¹¦ÄÜ£º PID¿ØÖÆÆ÷(Íâ»·)*******************************************************************************/voidControl_Angle(struct_out_angle*angle,struct_Rc*rc){ staticstruct_out_anglecontrol_angle; staticstruct_out_anglelast_angle;//////////////////////////////////////////////////////////////////// ÒÔÏÂΪ½Ç¶È»·////////////////////////////////////////////////////////////////// if(rc->ROLL>1490&&rc->ROLL<1510) rc->ROLL=1500; if(rc->PITCH>1490&&rc->PITCH<1510) rc->PITCH=1500;////////////////////////////////////////////////////////////////// if(rc->AUX1>1495&&rc->AUX1<1505) rc->AUX1=1500; if(rc->AUX2>1495&&rc->AUX2<1505) rc->AUX2=1500;////////////////////////////////////////////////////////////////// control_angle.roll=angle->roll-(rc->ROLL-1500)/13.0f+(rc->AUX2-1500)/100.0f; control_angle.pitch=angle->pitch-(rc->PITCH-1500)/13.0f-(rc->AUX1-1500)/100.0f;////////////////////////////////////////////////////////////////// if(control_angle.roll>angle_max) //ROLL egral+=angle_max; if(control_angle.roll<-angle_max) egral+=-angle_max; else egral+=control_angle.roll; if(egral>angle_integral_max) egral=angle_integral_max; if(egral<-angle_integral_max) egral=-angle_integral_max;////////////////////////////////////////////////////////////////// if(control_angle.pitch>angle_max)//PITCH egral+=angle_max; if(control_angle.pitch<-angle_max) egral+=-angle_max; else egral+=control_angle.pitch; if(egral>angle_integral_max) egral=angle_integral_max; if(egral<-angle_integral_max) egral=-angle_integral_max;////////////////////////////////////////////////////////////////// if(rc->THROTTLE<1200)//ÓÍÃŽÏСʱ£¬»ý·ÖÇåÁã { egral=0; egral=0; }////////////////////////////////////////////////////////////////// roll.output=roll.kp*control_angle.roll+roll.ki*egral+roll.kd*(control_angle.roll-last_angle.roll); pitch.output=pitch.kp*control_angle.pitch+pitch.ki*egral+pitch.kd*(control_angle.pitch-last_angle.pitch);////////////////////////////////////////////////////////////////// last_angle.roll=control_angle.roll; last_angle.pitch=control_angle.pitch;}#definegyro_max 50.0f #definegyro_integral_max 5000.0f/******************************************************************************º¯ÊýÔ­ÐÍ£º voidControl_Gyro(struct_SI_float*gyro,struct_Rc*rc,uint8_tLock)¹¦ÄÜ£º PID¿ØÖÆÆ÷(ÄÚ»·)*******************************************************************************/voidControl_Gyro(struct_SI_float*gyro,struct_Rc*rc,uint8_tLock){ staticstruct_out_anglecontrol_gyro; staticstruct_out_anglelast_gyro; int16_tthrottle1,throttle2,throttle3,throttle4;//////////////////////////////////////////////////////////////////// ÒÔÏÂΪ½ÇËٶȻ·////////////////////////////////////////////////////////////////// if(rc->YAW>1400&&rc->YAW<1600) rc->YAW=1500; if(rc->AUX3>1495&&rc->AUX3<1505) rc->AUX3=1500;////////////////////////////////////////////////////////////////// control_gyro.roll=-roll.output-gyro->y*Radian_to_Angle; control_gyro.pitch=pitch.output-gyro->x*Radian_to_Angle; if(rc->AUX4&Lock_Mode) control_gyro.yaw=-gyro->z*Radian_to_Angle-(rc->AUX3-1500)/100.0f;//Ëøβģʽ else control_gyro.yaw=-(rc->YAW-1500)/2.0f-gyro->z*Radian_to_Angle+(rc->AUX3-1500)/50.0f;//·ÇËøβģʽ////////////////////////////////////////////////////////////////// if(control_gyro.roll>gyro_max) //GYRO_ROLL gyro_egral+=gyro_max; if(control_gyro.roll<-gyro_max) gyro_egral+=-gyro_max; else gyro_egral+=control_gyro.roll; if(gyro_egral>gyro_integral_max) gyro_egral=gyro_integral_max; if(gyro_egral<-gyro_integral_max) gyro_egral=-gyro_integral_max;////////////////////////////////////////////////////////////////// if(control_gyro.pitch>gyro_max)//GYRO_PITCH gyro_egral+=gyro_max; if(control_gyro.pitch<-gyro_max) gyro_egral+=-gyro_max; else gyro_egral+=control_gyro.pitch; if(gyro_egral>gyro_integral_max) gyro_egral=gyro_integral_max; if(gyro_egral<-gyro_integral_max) gyro_egral=-gyro_integral_max;//////////////////////////////////////////////////////////////////// if(control_gyro.yaw>gyro_max)//GYRO_YAW// gyro_egral+=gyro_max;// if(control_gyro.yaw<-gyro_max)// gyro_egral+=-gyro_max;// else gyro_egral+=control_gyro.yaw; if(gyro_egral>gyro_integral_max) gyro_egral=gyro_integral_max; if(gyro_egral<-gyro_integral_max) gyro_egral=-gyro_integral_max;////////////////////////////////////////////////////////////////// if(rc->THROTTLE<1200)//ÓÍÃŽÏСʱ£¬»ý·ÖÇåÁã { gyro_egral=0; }////////////////////////////////////////////////////////////////// gyro_roll.output=gyro_roll.kp*control_gyro.roll+gyro_roll.ki*gyro_egral+gyro_roll.kd*(control_gyro.roll-last_gyro.roll); gyro_pitch.output=gyro_pitch.kp*control_gyro.pitch+gyro_pitch.ki*gyro_egral+gyro_pitch.kd*(control_gyro.pitch-last_gyro.pitch); gyro_yaw.output=gyro_yaw.kp*control_gyro.yaw+gyro_yaw.ki*gyro_egral+gyro_yaw.kd*(control_gyro.yaw-last_gyro.yaw);////////////////////////////////////////////////////////////////// last_gyro.roll=control_gyro.roll; last_gyro.pitch=control_gyro.pitch; last_gyro.yaw=control_gyro.yaw;////////////////////////////////////////////////////////////////// if(rc->THROTTLE>1200&&Lock==0) { throttle1=rc->THROTTLE-1050+gyro_pitch.output+gyro_roll.output-gyro_yaw.output; throttle2=rc->THROTTLE-1050+gyro_pitch.output-gyro_roll.output+gyro_yaw.output; throttle3=rc->THROTTLE-1050-gyro_pitch.output+gyro_roll.output+gyro_yaw.output; throttle4=rc->THROTTLE-1050-gyro_pitch.output-gyro_roll.output-gyro_yaw.output; } else { throttle1=0; throttle2=0; throttle3=0; throttle4=0; } Motor_Out(throttle1,throttle2,throttle3,throttle4);}A2模拟I'C驱动详解模拟I²C协议的GPIO口为PB10、PB11，主函数如下：int

main(void)Uartl_Init(115200)

//串口初始化Nvic_Init()

//中断分组设置I2C2_Int(）;

//IC初始化Delay(200);InitMPU6050();

//初始化

MPU6050while(1)Debug1_H;NPU6050_SequenceRead()

/连续读取

MPU6050数据寄存器Debug1_L;MPU6050_Compose()；

//MPU6050数据合成MPU6050_Printf();

//输出

MPU6050的原始数据Delay(100);串口初始化是少不了的，因为我们将通过它把读到的MPU6050原始数据发回PC端进行观察。Debugl_H和Debugl_L分别是让一个特定的GPIO口输出高电平和低电平，它的作用稍后介绍。I2C2_Int()如下：void

I2C2_Int(void)GPIO

InitTypeDef

GPIO

InitStructure;RCC_APB2PeriphC1ockCmd(RCC_APB2Periph_GPIOB

,ENABLE);

//打开外设B的时钟GPIO_InitStructure.GPIO_Pin

=

Debugl_Pin

|

Debug2_Pin

|

Debug3_Pin;//用于测量程序运行速率GPIO_InitStructure.GPIO_Speed

=

GPIO

Speed_50MHz;GPIO_InitStructure.GPIO_Mode

=

GPIO_Mode_Out_PP;

//推挽输出GPIO_Init(GPIOB,&GPIO_InitStructure);GPIO

InitStructure.GPIO

Pin

=

SCL

Pin;

//SCLGPIO_InitStructure.GPIO_Speed

=

GPIO_Speed_50MHz;GPIO_InitStructure.GPIO_Mode

=

GPIO_Mode_Out_OD;

//开漏输出GPIO_Init(GPIOB,&GPIO_InitStructure);GPIO_InitStructure.GPIO_Pin

=

SDA_Pin;

//SDAGPIO_InitStructure.GPIO_Speed

=

GPIO_Speed_50MHz;GPIO_InitStructure.GPIO_Mode

=

GPIO_Mode_Out_OD;

//开漏输出GP10_Init(GPIOB,5GPIO_InitStructure);PrintString("\r\n

模拟

12C初始化完成！”)：GPIO初始化函数，配置为开漏输出，这样既可以用作输出，可以读取GPIO口高电平的状态，而且不需要再改变GP1O的配置，优化了总线速度。Debug_Pin是用来量IC速率的，在读取总线开始和结束时翻转电平，既可以用逻辑分析仪测出FC通消耗的时间。经测量，连续读取14位寄存器在零延时只需要100多个ps.为了方便GPIO操作，给端口宏定义如下：#define

SCL_Pin

GPIO

Pin

10#define

SDA_Pin

GPIO

Pin

11#

defineSCL_HGPIOB->BSRR

=

SCL_Pin

/SCL高电平#

defineSCL

L

GPIOB->

BRR=

SCL_Pin

/SCL

低电平#

defineSDA

H

GPIOB

->

BSRR

=

SDA_Pin

//SDA高电平#defineSDA_L

GPIOB

->

BRR

=

SDA

Pin

//SDA低电平#

defineSDA_Read

GPIOB

-

>

IDR&

SDA_Pin

//SDA

读数据#defineDebugl_PinGPIO_Pin_5

//用于测量程序运行速率#defineDebug1_H

GPIOB->BSRR

=

Debugl_Pin

//高电平#define

Debugl

L

GPIOB

-

>

BRR=

Debugl_Pin

//低电平#defineDebug2_PinGPIO_Pin

9#def

ineDebug2_H

GPIOB->BSRR

=

Debug2_PinGPIOB->BRR#defineDebug2_L

=Debug2_Pin#defineDebug3

PinGPIO_Pin_8#

defineDebug3_H

GPIOB->BSRR

=

Debug3_Pin#define

Debug3_L

GPIOB

->

BRR=Debug3_Pin用GPIO模拟PC时序，需要注意的是：在传输数据的时候，SDA线必须在时钟的电平周期保持稳定；SDA的高或低电平状态只有在SCL线的时钟信号为低电平时能改变。SCL线是高电平时，SDA线从高电平向低电平切换，这个情况表示开始条件，若起FC总线失败，则返回0；若成功，则返回1。static

uint8_t

I2C

Start(void)SCL_L;SDA_H;SCL

H;if(!SDA_Read)return

0;

//SDA

线为低电平则总线忙,加入SDA_L;if(SDA_Read)return

0;

//SDA线为高电平则总线出错,加入SDA

L；return

1;SCL线是高电平时，SDA线由低电平向高电平切换，这个情况表示停止条件：static

void

12C_Stop(void)SCL_L：5DA

L:SCL

H;SDA_H;开始和停止条件一般由主机(这里的主机是STM32)产生。总线在起始条件后被认为处于忙的状态，在停止条件的某段时间后总线被认为再次处于空闲状态。如果产生重复起始条件而不产生停止条件，则总线会一直处于忙的状态，此时的起始条件和重复起始条件在功能上是一样的。字节式子：发送到SDA线上的每个字节必须为8位，每次传输可以发送的字节数量不受限制。每个字节后必须跟一个响应位。首先传输的是数据的最高位(MSB)，这里用GPIO模拟时序向FC总线写1字节数据，数据高位在前，低位在后：static

void

I2C_

WriteByte(unsigned

char

SendByte)uintB_ti=8;while(i--)SCL_L;if(

SendByte&0x80)

SDA_H;else

SDA_L;SendByte<<=1;SCL

H;SCL_L;这里用GPIO模拟时序，向PC总线读1字节数据，数据高位在前，低位在后：static

uint8_t

I2C_ReadByte(void)uint8_ti=8;uint8_t

ReceiveByte

=0;SDA_H;while(i--)SCL

_L;nop()；SCL_H;ReceiveByte<<=1;if(SDA_Read)ReceiveByte

|

=

0x01;SCL

L;return

ReceiveByte;应答响应：数据传输必须带响应，相关的响应时钟脉冲由主机产生。在响应的时钟脉冲期间发送器释放SDA线(置高电平)，在响应的时钟脉冲期间，接收器必须将SDA线拉低，使它在这个时钟脉冲的高电平期间保持稳定的低电平。通常被寻址的接收器在接收到的每个字节后，必须产生一个响应，当从机不能响应从机地址时(例如它正在执行一些及时函数不能接收或发送)，主机将产生一个停止条件停止传输或者产生重复开始条件开始新的传输，从机必须使数据线保持高电平。如果从机接收器响应了从机地址，但是在传输了一段时间后不能接收更多数据字节，主机必须再一次停止传输，这个情况用从机在第一个字节后没有产生响应来表示。从机使数据线保持高电平，主机产生一个停止或重复开始条件。这里用GPIO模拟时序接收从机的ACK/NACK信号：static

uint8_t

I2C_NaitAck(void)SCI._L;SDA

8;SCL_H;f(SDA_Read)SCL_L;return

0;SCL_L；return

1;如果传输中有主机接收器，则它必须在从机发出的最后一个字节时产生一个响应，向从机发送器通知数据结束。从机发送器必须释放数据线，允许主机产生一个停止或重复开始条件。根据时序要求，模拟主机应答响应(ACK)及无应答响应(NACK)信号如下：static

void

I2C_ACK(void)SCL_L;___nop()；SDA_L;

//写应答信号__nop()；SCL_H;__nop()；SCL

L;static

void

I2C

NACK(void)SCL

L;SDA_H;

//不写应答信号SCL_H;SCL_L;至此，PC的基本操作已经模拟结束，接下来可以实现对PC从机(MPU6050)的读/写操作了。根据上述要求，对FC是否忙进行了判断：若PC总线开始错误，则退出总线：若FC从机(MPU6050)无ACK应答，则也退出总线操作。以下是MPU6050的写操作函数：static

uint8_t

Single_NriteI2C(unsigned

char

Regs_Addr,unsigned

char

Regs_Data)if(!I2C

Start())return

0;

//1C起始错误，返回I2C

WriteByte(MPU6050Address)；//写从机地址，并配置成写模式if(!I2C_WaitAck())I2C_Stop(

);return

0;

//无ACK，返回I2C_WriteByte(Regs_Addr);

//写寄存器地址I2C_WaitAck();12C

WriteByte(Regs_Data);

//写寄存器数据I2C_WaitAck();I2C_Stop()；return

1;同理，以下是MPU6050的读操作函数：#def

ine

MPU6050Address

OxD0static

uint8_t

Single_ReadI2C(unsigned

char

Regs_Addr)uint8_t

ret;if(!I2C_Start())return

0;

//I²c开始错误,返回I2C_WriteByte(MPU6050Address);

//写从机地址，并配置成写模式if(!I2C_WaitAck())I2C_Stop();return

0;

//无ACK,返回I2C_WriteByte(Regs_Addr);

//写寄存器地址I2C_WaitAck();I2C

Start();I2C_WriteByte(MPU6050Address+1);

//写从机地址，并配置成读模式I2C_WaitAck();ret

=

I2C_ReadByte();

//从传感器中读出数据I2C_NACK();

//无应答I2C_Stop()；

//结束本段I²C进程return

ret;A3姿态解算代码以下是姿态解算的主要程序：//========================================================//描述：//必须定义halfT'为周期的一半，以及滤波器的参数kP和kI//四元数'q0'，'q1'，'q2，'q3'定义为全局变量//需要在每一个采样周期调用'IMUupdate()'函数//陀螺仪数据单位是弧度/秒，加速度计的单位无关重要，因为会被规范化//========================================================#

include

<math.h>#definek

P1.6f//比例常数#definekI

0.001f//积分常数#define

halfT

0.0005f//半周期#define

T

0.001f//周期为1ms//变量定义loat

q0=1,

ql

=0，q2

=0，q3=0://四元数，定义为全局变量float

exInt

=0,

eyInt

=

0.

ezInt

=0://误差积分累计值函数原型：void

IMlupdate(float

gx,

float

gy.

float

gz,

float

ax.

loatay.

float

az)//功能：互补滤波进行姿态解算输入：陀螺仪数据及加速度计数据void

IMUupdate(float

gx.

float

gy,

float

gz,

float

ax,

loatay.

float

az)float

norm;float

vx,

vy,

vz;float

ex.

ey.

ez;//把加速度计的三维向量转成单位向量norm

=1.0f/sqrt(ax*ax

+

ayay

+

az*az);ax

=

ax

*

norm;ay

=

ay

*

norm;az

=

az

*

norm;//估计重力加速度方向在飞行器坐标系中的表示，为四元数表示的旋转矩的第三行vx=2*(ql*q3

-

q0*q2);vy=2*(q0*ql+q2

*q3);vz

=q0*q0-q1*ql

-

q2*q2

+

q3*q3;//加速度计读取的方向与重力加速度方向的差值，用向量又乘计算ex

=

ay

vz

-

az

vy;ey

=

az+

vx

-

ax

vz;ez

=

axvy

-

ay

vx;//误差累积，已与积分常数相乘exInt

=

exint

+

ex+kI;eyInt

=

eyInt

+

ey*kI;ezInt

=

ezInt

+

ez*kI;//用叉积误差来做PI更正陀螺零偏，即对消陀螺读数中的偏移量gx

=

gx

+

kPxex

+

exint;gy

=

gy

+

kP*ey

+

eyInt;gz

=

gz

+kP*ez

+

ezInt;//四元数积分，求得当前的姿势float

q0_last=

q0;float

ql_last=

ql;float

q2_last

=

q2;float

q3_last

=

q3;//一阶近似算法，即为四元数运动学方程的离散化形式和积分q0

=

q0_last

+

(-ql_last

*

gx

-

q2_last*gy

-

q3

last*z)×halfT;ql

=

ql_last

+

(q0_last*gx

+

q2_last*gz

-

q3last*gy)*balfT;q2

=

q2_last

+

(q0_last*gy

-

ql_last

gz

+q3_last+

gx)*balfT;q3

=q3_last

+

(q0_last*gz

+

ql_last+gy-

q2_last*gx)halfT;//二阶近似算法

gy

*gy

+

gz×gz)*T*T//float

delta2=(gx×gx+//q0

q0

last(1-delta2/8)+(-q1_last*gx-q2_last*gy

q3_lastgz)xhlfT;//ql

2(1ast”(1-delta2/8)+(q0_

last

"

gx

q2_last

·gz

-q2.Jm*gy)*halfT;//q2

=(2

last*(1-delta2/8)+(q0_last

*gy

-q1_last

*

gz

+

q)lam×gx)*halfTi;//q3=q3.1ast+(1-delta2/8)+(q0

lont

*ge+

q1_last

*

gy

-92.Jst*gx)×halfT;//三阶近似算法//float

delta2=(gxgx+

gy×gy

+

gz*gz)*T*T;//q0

=

q0_last*(1-delta2/8)×gz)*T*(0.5-delta2/48)；9Y-q3_last+(q0_last*gx

+

q2_last*gz

-q3_lastql

=

ql_last*(1-delta2/8)*gy)*T×(0.5-delta2/48);//q2

=

q2_last×(1-delta2/8)+(q0_last*gy

-

ql_last*gz

+

q3_last*gx)*T×(0.5-delta2/48);//q3

=

q3_last*(1-delta2/8)+(q0_last

*gz

+

ql_last*gy

-q2_last*gx)*T*(0.5-delta2/48);//四阶近似算法//float

delta2=(gx

gx

+

gy*gy+

gz*g2)*T*T;//q0

=

q0_last*(1-

delta2/8

+

delta2*delta2/384)+(-q1

last*gx-

q2_last

*gy

-

q3_last*gz)*T*(0.5-

delta2/48)//ql

=

ql_last*(1

-

delta2/8

+

delta2×delta2/384)+(q0_last*gx

+

q2_last*gz

-q3_last

*

gy)*T*(0.5

-

delta2/48);//q2

=

q2_last*(1-

delta2/8

+

delta2×delta2/384)+(q0_last*gy

-

ql_last*gz

+

q3_last*

gx)*T*(0.5

-

delta2/48);//q3

=

q3_last*(1

-

delta2/8

+

delta2*

delta2/384)+(q0_last*gz

+

ql_last*gy

-

q2_last*gx)*T*(0.5-

delta2/48);//四元数规范化norm

=1/sqrt(q0*q0+ql×ql+q2

*q2

+

q3

*q3)；q0

=

q0

*

norm;ql=q1

*

norm;q2

=

q2

*

norm;q3

=

q3

*

norm;//函数原型：void

Get_Euler_Angle(struct

EulerAngle

*angle)//功能：四元数转欧拉角//注意：若采用不同的欧拉角顺序，则代码会不一样void

Get_Euler_Angle(struct

EulerAngle

×angle)angle->roll

=atan2(2.Of*(q0*q1+q2×q3)q0-2-q1-2-q2-2+q3-2)×180/piangle->pitch

=

asin(2.Of*(q0*q2-

ql*q3))*180/pi;angle->yaw=

atan2(2*(q0*ql+q2×q3)，q0-2+q1-2-q2-2-q3-2)*180/piiA4蓝牙部分代码允许程序连接到已配对的蓝牙设备。允许程序发现和配对蓝牙设备初始化蓝牙BluetoothAdapter

mBluetoothAdapter

=

BluetoothAdapter.getDefaultAdapter();if

(null

==

mBluetoothAdapter)

{Toast.makeText(this.getApplicationContext(),

R.string.not_support_bluetooth,

Toast.LENGTH_SHORT).show();return;}开启蓝牙if(!mBluetoothAdapter.isEnabled()){Toast.makeText(MainActivity.this,

getString(R.string.open_bluetooth),

2000).show();return;}或者mBluetoothAdapter.enable()搜寻蓝牙mBluetoothAdapter.startDiscovery();//TODO:搜寻蓝牙之前需要判断定位权限和GPS开关

MIUI权限需要定制处理if

(PermissionUtil.getInstance().checkGPSPermission(MainActivity.this))

{if

(!AppUtil.isGpsOPen(MainActivity.this)

&&

PermissionUtil.getInstance().isOverMarshmallow())

{permissionDialog

=new

PermissionDialog(MainActivity.this,

R.style.Dialog);permissionDialog.setOnShowListener(new

DialogInterface.OnShowListener()

{@Override

p