[从零开始]做四轴

最新推荐文章于 2022-05-17 17:23:42 发布

老衲开心用飘柔

最新推荐文章于 2022-05-17 17:23:42 发布

阅读量2.4k

点赞数 3

分类专栏：四轴文章标签：从零开始四轴学习记录

四轴专栏收录该内容

1 篇文章 0 订阅

订阅专栏

原文链接：http://www.9mcu.com/9mcubbs/forum.php?mod=viewthread&tid=1047581&page=1&authorid=403735

关于陀螺仪视频可参考原文链接

本贴会持续,不断更新,楼主会跟着大家一同开始四轴的学习.

上位机的数据处理速度可以1毫秒处理一个几十字节的数据帧(无线数据包为32字节,这里就以32字节为例),也就是说1秒可以处理32000字节的数据. (1ms接收32字节的数据,并分析,显示数据波形)  这里可以说是写上位机时最难的,占据了大量的时间,因为如果做到10ms接收一帧数据并显示,是比较容易的,如果频率快了,比如现在的1ms一帧数据的话,pc的数据处理和显示刷新要处理的很好才行,要不程序就很容易"死掉",因为windows不是实时系统,还有很多事情要做,不能把所有资源都给上位机程序. 目前传感器的采样频率也是1ms一次,这样就可以用上位机直接观察采样值和滤波后的值,很方便,可以实时观察滤波效果,不用再把数据先保存在内存卡或者电脑上,飞完了在调出来看波形了.
而且听取大家意见,很多人没有航模遥控,这里加上了鼠标键盘控制功能,可以用鼠标和键盘控制飞机了,当然,手感肯定没有遥控好了.

为了减轻重量,飞机上没有设计遥控接收模块,飞机的遥控通过飞控上自带的NRF24L01芯片完成,同时该芯片还负责数据的传输,所以飞机不能直接通过商业遥控遥控飞行,需要用一个单片机采集遥控信号再通过NRF模块发给飞机来完成遥控,同时飞机可以将飞行数据通过NRF模块发给单片机,单片机再通过串口发送到pc上位机,上位机也可以把飞行参数通过NRF模块实时发给飞机.等于是同时存在以下几个数据连接:
1:遥控->遥控板->飞机
2:PC上位机->遥控板->飞机
3:飞机->遥控板->PC上位机
可以看出所有的数据传输都要通过遥控板.这样的好处是在遥控飞行的同时可以用上位机接收飞机的飞行数据,并在上位机实时显示,上位机也可以在飞行过程中或飞机落地后,修改飞控参数,十分方便.

焊接好后,就要开始软件调试了,这里才是四轴最"好玩"的地方,当初决定玩四轴也是因为这个,因为四轴硬件简单,主要拼的是软件,不像直机什么的,硬件太复杂了.
软件的调试,建议大家还是一步一步来.虽然我开源了飞控的全部程序,大家烧进去就可以实现基本的飞行了,但是我们自己做飞控最关键的就是程序了,直接copy一个工程过来,想必大家直接消化还是很不容易的.就像学习骑车一样,还不会走路,就学骑自行车,怎么会骑好.所以还是建议大家从基础的开始.

从哪里开始呢,因为我们使用的是stm32f103,大家就从最简单的stm32的驱动开始吧.
以下都是针对我的飞控板写的,大家如果用的自己的,记得需要把IO和端口都改为自己的
stm32可以说就是飞控的大脑,mpu6050和hmc5883就是飞控的眼睛,耳朵.飞控的所有功能,都是通过stm32实现的,所以大家清楚了stm32做什么了以后,就要好好的把他驱动好,让他按照我们的想法来运转,这样飞控才能好好的工作.
首先下载我们需要的软件环境,就是MDK (KEIL FOR ARM),我用的是4.7版本,有了自动补全功能,挺好用.当然,还要有个JLINK,使用swd方式进行调试.然后就可以开始第一步的程序编写了.按照网上的教程,使用库函数建立一个空白的工程,从最简单的,点亮一个LED开始.
要点亮一个led,也不是一件简单的事情,首先,系统时钟要配置好(最新库函数在初始化时已经帮我们把时钟初始化好了,不用我们再初始化了,当然,除非你有特殊要求),然后要配置端口的输入输出,还要打开响应端口的外设时钟.
void LED_INIT(void)
{
      GPIO_InitTypeDef GPIO_InitStructure;       这个是端口配置结构体
      RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);          打开外设GPIOB的时钟,因为LED是通过GPIOB的9,11,14,15来驱动的
      GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_14 | GPIO_Pin_15 | GPIO_Pin_9 ;                选择响应的管脚
      GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                设置IO速度
      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;                设置IO驱动方式,out为输出,PP为推挽输出
      GPIO_Init(GPIOB, &GPIO_InitStructure);                                        对GPIOB进行初始化
}
初始化好了,再写几个函数来操作LED,更加方便
#define LED1_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_11)
#define LED1_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_11)
#define LED2_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_14)
#define LED2_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_14)
#define LED3_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_15)
#define LED3_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_15)
#define LED4_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_9)
#define LED4_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_9)
#define LEDALL_OFF  GPIO_SetBits(GPIOB, GPIO_Pin_11 | GPIO_Pin_14 | GPIO_Pin_15 | GPIO_Pin_9)
#define LEDALL_ON       GPIO_ResetBits(GPIOB, GPIO_Pin_11 | GPIO_Pin_14 | GPIO_Pin_15 | GPIO_Pin_9)

这几个函数功能很简单,就是对IO的输出电平进行操作.当LED可以点亮后,更高级的比如LED的闪烁,就需要大家自己写了,实现的方法很多,可以用循环实现,也可以用定时中断实现.选用哪种方式实现,就要看具体的使用地点和情况了.

很多童鞋已经拿到了套件,由于基础不同,大家在硬件焊接/模块驱动/程序调试等方面会遇到各种各样的问题,为了给大家解答,也为了方便大家少走弯路,特此开贴,从最开始的硬件焊接调试,到最后的飞行调试,一一给大家介绍,争取让大家跟着教程都能做出自己的飞控.(当然楼主也是刚开始做不久,飞机也只是可以飞行,还是很不稳定,还需要不断学习,努力)
本贴会持续,不断更新,楼主会跟着大家一同开始四轴的学习.

上位机的数据处理速度可以1毫秒处理一个几十字节的数据帧(无线数据包为32字节,这里就以32字节为例),也就是说1秒可以处理32000字节的数据. (1ms接收32字节的数据,并分析,显示数据波形)  这里可以说是写上位机时最难的,占据了大量的时间,因为如果做到10ms接收一帧数据并显示,是比较容易的,如果频率快了,比如现在的1ms一帧数据的话,pc的数据处理和显示刷新要处理的很好才行,要不程序就很容易"死掉",因为windows不是实时系统,还有很多事情要做,不能把所有资源都给上位机程序. 目前传感器的采样频率也是1ms一次,这样就可以用上位机直接观察采样值和滤波后的值,很方便,可以实时观察滤波效果,不用再把数据先保存在内存卡或者电脑上,飞完了在调出来看波形了.
而且听取大家意见,很多人没有航模遥控,这里加上了鼠标键盘控制功能,可以用鼠标和键盘控制飞机了,当然,手感肯定没有遥控好了.

为了减轻重量,飞机上没有设计遥控接收模块,飞机的遥控通过飞控上自带的NRF24L01芯片完成,同时该芯片还负责数据的传输,所以飞机不能直接通过商业遥控遥控飞行,需要用一个单片机采集遥控信号再通过NRF模块发给飞机来完成遥控,同时飞机可以将飞行数据通过NRF模块发给单片机,单片机再通过串口发送到pc上位机,上位机也可以把飞行参数通过NRF模块实时发给飞机.等于是同时存在以下几个数据连接:
1:遥控->遥控板->飞机
2:PC上位机->遥控板->飞机
3:飞机->遥控板->PC上位机
可以看出所有的数据传输都要通过遥控板.这样的好处是在遥控飞行的同时可以用上位机接收飞机的飞行数据,并在上位机实时显示,上位机也可以在飞行过程中或飞机落地后,修改飞控参数,十分方便.

焊接好后,就要开始软件调试了,这里才是四轴最"好玩"的地方,当初决定玩四轴也是因为这个,因为四轴硬件简单,主要拼的是软件,不像直机什么的,硬件太复杂了.
软件的调试,建议大家还是一步一步来.虽然我开源了飞控的全部程序,大家烧进去就可以实现基本的飞行了,但是我们自己做飞控最关键的就是程序了,直接copy一个工程过来,想必大家直接消化还是很不容易的.就像学习骑车一样,还不会走路,就学骑自行车,怎么会骑好.所以还是建议大家从基础的开始.

从哪里开始呢,因为我们使用的是stm32f103,大家就从最简单的stm32的驱动开始吧.
以下都是针对我的飞控板写的,大家如果用的自己的,记得需要把IO和端口都改为自己的
stm32可以说就是飞控的大脑,mpu6050和hmc5883就是飞控的眼睛,耳朵.飞控的所有功能,都是通过stm32实现的,所以大家清楚了stm32做什么了以后,就要好好的把他驱动好,让他按照我们的想法来运转,这样飞控才能好好的工作.
首先下载我们需要的软件环境,就是MDK (KEIL FOR ARM),我用的是4.7版本,有了自动补全功能,挺好用.当然,还要有个JLINK,使用swd方式进行调试.然后就可以开始第一步的程序编写了.按照网上的教程,使用库函数建立一个空白的工程,从最简单的,点亮一个LED开始.
要点亮一个led,也不是一件简单的事情,首先,系统时钟要配置好(最新库函数在初始化时已经帮我们把时钟初始化好了,不用我们再初始化了,当然,除非你有特殊要求),然后要配置端口的输入输出,还要打开响应端口的外设时钟.
void LED_INIT(void)
{
      GPIO_InitTypeDef GPIO_InitStructure;       这个是端口配置结构体
      RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);          打开外设GPIOB的时钟,因为LED是通过GPIOB的9,11,14,15来驱动的
      GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_14 | GPIO_Pin_15 | GPIO_Pin_9 ;                选择响应的管脚
      GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                设置IO速度
      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;                设置IO驱动方式,out为输出,PP为推挽输出
      GPIO_Init(GPIOB, &GPIO_InitStructure);                                        对GPIOB进行初始化
}
初始化好了,再写几个函数来操作LED,更加方便
#define LED1_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_11)
#define LED1_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_11)
#define LED2_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_14)
#define LED2_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_14)
#define LED3_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_15)
#define LED3_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_15)
#define LED4_OFF       GPIO_SetBits(GPIOB, GPIO_Pin_9)
#define LED4_ON                GPIO_ResetBits(GPIOB, GPIO_Pin_9)
#define LEDALL_OFF  GPIO_SetBits(GPIOB, GPIO_Pin_11 | GPIO_Pin_14 | GPIO_Pin_15 | GPIO_Pin_9)
#define LEDALL_ON       GPIO_ResetBits(GPIOB, GPIO_Pin_11 | GPIO_Pin_14 | GPIO_Pin_15 | GPIO_Pin_9)
这几个函数功能很简单,就是对IO的输出电平进行操作.当LED可以点亮后,更高级的比如LED的闪烁,就需要大家自己写了,实现的方法很多,可以用循环实现,也可以用定时中断实现.选用哪种方式实现,就要看具体的使用地点和情况了.

串口初始化好了,就要测试收发了,将串口用串口线和电脑连接起来,先来简单的发送,用上位机看看能不能收到数据.
对了这里还要设置串口中断
      NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

      NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
      NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
      NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
      NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
      NVIC_Init(&NVIC_InitStructure);
我用的第二组,串口给了优先级3,subfriority3

串口中断函数:
void USART1_IRQHandler(void)
{
      USART1_IRQ();
}
void USART1_IRQ(void)
{
      if(USART_GetITStatus(USART1,USART_IT_TXE)!=RESET)
      {
            发送中断
      }
      if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
      {
            接收中断
      }
}

中断也写好后,基本的发送接收应该就没问题了.
但是在使用过程中,没有一个缓冲区肯定是不行的,关于这方面的帖子很多,大家在论坛上搜索下吧.我就简单介绍下我用的简单的环形缓冲吧.
我用了一个环形发送缓冲,比如一个长度为100字节的数组,再定义两个变量,比如num与t_num
num表示缓冲区内下一个空字节在第几字节,t_num表示下一个需要发送的字节在缓冲区的多少字节
比如需要发送12345 这5个数字,分别写入数组的01234字节,共5字节,此时num+=5,表示buf[5]是空的.写入数据后,激活发送,发送完一个字节,t_num+1,并判断t_num与num的大小,如果t_num<num,就急需发送,如果t_num=num,说明数据已经发送完了.
大致意思就是这样,不知道我表达清楚了没有.

串口做好了,再做点什么呢,就Timer吧,这个是以后程序里很重要的一个内容,因为timer是用来计时的,我们以后要用它来触发做很多事情,比如定时去读传感器并控制电机等等.
还是一样,先初始化Timer,以TIMER3为例
void TIM3_INIT(void)
{
      TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
         初始化使用的结构体
      RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
         打开timer3的时钟

      TIM_DeInit(TIM3);
      将timer3寄存器设置为默认
      TIM_TimeBaseStructure.TIM_Period=1000;    这个寄存器我理解是比较值,就是用这个值来和cnt比较,判断是否溢出(大于这个数就溢出)

      TIM_TimeBaseStructure.TIM_Prescaler=72-1;  这个寄存器我理解是分频值,就是timer3的时钟除以这个值+1就是计数器的时钟,这里72分频,就是1M,刚才比较值设置为1000,那么溢出的频率就是1000HZ,也就是每次中断1MS

      TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;       采样分频

      TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up;       向上计数

      TIM_TimeBaseInit(TIM3,&TIM_TimeBaseStructure);                            时钟初始化

      TIM_ClearFlag(TIM3,TIM_FLAG_Update);                                  清楚标志

      TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);                         开中断

      TIM_Cmd(TIM3,ENABLE);                                                    使能timer3
}

有人让我解释一下我程序里面PID计算的过程,这里就插一节吧,就是介绍下我的PID,当然,还很不完善,后期肯定还要经过很多改动才行,这里就先介绍下现在的"初级版"吧.
void PID_CAL(void)             PID计算函数
{
      static float thr=0,rool=0,pitch=0,yaw=0;       控制量
      static float rool_i=0,pitch_i=0;                      积分

      int16_t Motor1,Motor2,Motor3,Motor4;          四个电机输出

      IMU_TEST();                                                 计算姿态
      GET_EXPRAD();                                           获得控制量

      rool       = PID_RP.P * DIF_ANGLE.X;                   roll方向的P计算

      if(Q_ANGLE.Rool>-0.1 && Q_ANGLE.Rool<0.1) 判断I是否需要清零
      {
            rool_i = 0;
      }
      rool_i -= PID_RP.I * Q_ANGLE.Rool;                I计算
      PID_RP.IMAX = DIF_ANGLE.X * 10;                   积分限幅
      if(PID_RP.IMAX<0)
      {
            PID_RP.IMAX = (-PID_RP.IMAX) + 100;
      }
      else
      {
            PID_RP.IMAX += 100;
      }
      if(rool_i>PID_RP.IMAX)       rool_i = PID_RP.IMAX;
      if(rool_i<-PID_RP.IMAX)       rool_i = -PID_RP.IMAX;
      rool += rool_i;                                                          积分

      rool -= PID_RP.D * GYRO_F.X;                                     D计算
///
      pitch       = PID_RP.P * DIF_ANGLE.Y;                               同上

      if(Q_ANGLE.Pitch>-0.1 && Q_ANGLE.Pitch<0.1)
      {
            pitch_i = 0;
      }
      pitch_i -= PID_RP.I * Q_ANGLE.Pitch;
      if(PID_RP.IMAX<0)
      {
            PID_RP.IMAX = (-PID_RP.IMAX) + 100;
      }
      else
      {
            PID_RP.IMAX += 100;
      }
      if(PID_RP.IMAX<0)       PID_RP.IMAX = -PID_RP.IMAX;
      if(pitch_i>PID_RP.IMAX)       pitch_i = PID_RP.IMAX;
      if(pitch_i<-PID_RP.IMAX)       pitch_i = -PID_RP.IMAX;
      pitch += pitch_i;

      pitch -= PID_RP.D * GYRO_F.Y;
/
      GYRO_I[0].Z += EXP_ANGLE.Z/3000;                            yaw方向就简单的用了陀螺的积分 PD
      yaw = -10 * GYRO_I[0].Z;

      yaw -= 3 * GYRO_F.Z;
//
      thr = RC_DATA.THROTTLE+400;
将控制量输出给电机
      Motor1=(int16_t)(thr + rool - pitch + yaw);
      Motor2=(int16_t)(thr + rool + pitch - yaw);
      Motor3=(int16_t)(thr - rool + pitch + yaw);
      Motor4=(int16_t)(thr - rool - pitch - yaw);
      if(FLY_ENABLE && (RC_DATA.THROTTLE>-400))                         和解锁有关,未解锁或油门太低电机禁止转动
            MOTO_PWMRFLASH(Motor1,Motor2,Motor3,Motor4);
      else
            MOTO_PWMRFLASH(0,0,0,0);
}
程序就这么简单,当然还需要很多改进,PID参数也需要慢慢调试才行

串口/TIMER都讲过了,这次讲点什么呢,稍微提一提怎么调试吧,因为今天群里有人问我串口是用来做什么的?这个问题让我一下懵了.这该怎么回答?
串口,在我们这个工程里,其实就是用来调试的,无线控制是通过SPI来进行,其实无线调试好以后,串口也就没有用了,可以使用无线进行调试的,这样更加方便.其实串口是完全可以省略的.
那么怎么使用这个串口呢?串口可以理解为一根网线,通过他,可以将飞机上的数据传输到电脑上,就这么简单.........至于传什么,怎么传,学问就大了....
串口驱动好以后,可以帮助我们分析很多问题,有些甚至JLINK也做不到,观察变量,观察标志位,观察程序运行状态,等等,都可以通过串口来实现,上位机为了配合串口和无线调试,还特意做了一个DEBUG功能,就是为了帮助大家更方便的调试,可以通过串口发送一个自定义的命令,打开上位机的LED,或者改变上位机显示的寄存器数字.这样可以帮助大家知道程序的执行状态,比如中断进入没进入啊,进入的频率啊,变量现在的值是多少啊等等信息.
我这里仅仅是一点点自己的调试经验,至于怎么发送,发送一个什么样的格式,大家个人习惯不同,写出来当然也各不相同,有人喜欢用printf,我就喜欢十六进制,各有长短

可能一个模块一个模块的讲有些啰嗦,而且这些模块的使用方法网上也有很多教程,我以后就不在说这个了.大家有问题就搜索下吧,很容易找到.我目前用到的外设有:timer,usart,iic,spi,flash,gpio,exit,暂时就这么多.
今天想跟大家谈谈加速度计和陀螺仪,不少人在问加速度计陀螺仪的数据读出来了怎么用,咱们就从这两个传感器的特点开始了解下,了解了特点,用法就很容易了.
以下仅代表个人观点,有哪里不对,还请指出....
做个比喻吧,加速度计,以下简称加计,大家可以把它想象成一个铁块,这个铁块是个立方体,有前后左右上下六个面,每个面连接有一个弹簧,弹簧另一端假设固定在一个卡车的集装箱里面,这样这个铁块就被这六个弹簧吊在集装箱里面了,由于铁块有重力,所以汽车不动时,上面的弹簧被拉长,下面的弹簧被压缩,这里假设是通过测量弹簧的拉力来输出加速度(实际有可能是电容什么的,这里不做讨论,了解特性就好),六个弹簧,两两一组,正好3个轴,这就是3轴加速度了,静止不动时,只有Z轴也就是上下两根弹簧有读数,其他两对弹簧是平衡的.现在假设汽车在做加速运动,那么不仅仅上下两根弹簧不平衡了,前后两根弹簧也会有变化,前面的弹簧拉长,后面的弹簧压缩,就有了前后方向的加速度.左右也是一个道理.
知道了加计的大致原理,那么加计有什么特点呢?让我们大家想象一种情况,就是这辆卡车行驶在颠簸的路上,集装箱里面的铁块肯定不会稳稳的吊着了,他会随着汽车左摇右摆,上下颠簸,而且有一点大家注意,铁块的此时的摆动,不是完全和汽车同步的,由于惯性等原因,铁块会在里面"乱动",荡来荡去,此时的加速度输出会是怎么样的呢?肯定也是随着铁块"荡来荡去",所以我们得出加计的一大特点,就是对震动很"敏感",如果把飞控板放在桌子上静止不动,可以说随便一个姿态算法的输出都不错,哪怕不滤波.可以当电机一转动起来,震动来了,加计就有了很大的干扰,此时如果处理不好,姿态就乱掉了.
然后我们再说说陀螺仪,陀螺仪顾名思义,肯定和陀螺有很大关系,没错,特点也和陀螺一样.还是假设在这个车里面,我们放上一个小时候玩的陀螺,不管用了什么方法,让它高速旋转起来,大家都知道,这样陀螺是不会倒的,他会尽量保持当前的姿态,陀螺仪正是利用这个特点.我们看两段视频来了解下.

今天跟大家谈一谈6050的数据吧,首先,量程,
GYRO: ±250, ±500, ±1000, and ±2000°/sec (dps). 这是什么意思呢,就是陀螺数据在输出32767(也就是int16型数据的最大值)的时候,分别表示250度每秒一直到2000度每秒.
ACC: ±2g, ±4g, ±8g, and ±16g. 表示输出32767时表示2倍重力加速度一直到16倍重力加速度.
量程选的小,数据就准确一些(在量程范围内),但是在剧烈动作时,容易超调.
知道了量程和数据的意义,怎么把输出的数据转成角度想必就很简单了吧.
acc的数据出来后,进行一个移动平均值滤波,作用就是个低通滤波,gyro的数据换成角度,积分,就可以得到"姿态角",这里直接得到的姿态角是不对的,要对三轴数据进行"解耦"---应该是这么叫,这个我就不太懂了,大家可以参考一下mwc的程序或者网上寻找教程.
得到了角度,定期用acc的角度来矫正陀螺的误差,就可以实现自稳这个功能了.

今天抽出点时间,再跟大家聊一聊.
其实如果把教程前面的东西消化了,到现在应该已经可以得到自己的传感器读数和简单的姿态数据了,今天就里面的细节问题聊一聊吧.
做飞控，最重要的应该要属传感器读数的准确性和姿态的算法了，之前讲了加速度和陀螺仪传感器，希望大家在睡觉前可以闭上眼想象一下，这俩传感器怎么配合才合适，构思一下算法，这里的构思，只要一个大题思路就行，不可能在脑子里就把程序一句句写好吧。。。
有个问题，我们什么时候去读传感器呢？我觉得这样看两个情况，一：系统速度够快，比如6050的采样周期是1ms，我的单片机在1ms里面可以完成数据读取、滤波、姿态计算、输出等计算过程，那么，我就可以使用定时器中断来触发读取，然后是依次滤波。。。。。二：系统不够快，有的飞控速度慢，1ms完成不了那么多任务，很多浮点运算对8位机确实任务有点重，那怎么办？那我们就循环读，什么时候完成计算过程了，什么时候读取传感器数据，并开始下一次计算过程，这样不浪费单片机的时间。
有点点调试经验，跟大家分享下，如果大家也和我用的单片机一样，都是stm32f103，想必大家在调试IIC的时候也遇到了各种各样的困难，确实，stm32的IIC是个很蛋疼的东西，当然，我是针对的硬件，模拟的还是很好用的。当时调试IIC，那可是尝试了各种方法，中断，dma，官方的cpal什么的，就一个字，不好用。。。。。当时都想外加个单片机负责IIC的读取了。。。。。如果大家要调试硬件的IIC，我有几个小经验，不能用变量观察窗口，并在IIC程序中设置断点，比方说我想看看SR1或者SR2寄存器什么状态呀，不要想了，不能这样，因为stm IIC相关寄存器很多都是读取后清零，大家用Jlink看寄存器，就相当于读取了，寄存器清零，不该清零的时候清零，后果很严重，IIC直接挂掉了。。。。。STM的模拟IIC还是很稳定的，相关程序请参考我的工程。

现在假设大家已经得到了一下数据：
滤波后的加速度数据 AX,AY,AZ，陀螺仪数据：GX,GY,GZ，怎么用这些数据得到姿态？
我的程序里面用的四元数，我就简单给大家讲讲用法，具体四元数的原理什么的，我的不懂。。。。。矩阵、旋转、空间、三维。。。。实在太。。
IMUupdate(GX,GY,GZ,AX,AY,AZ);
这个函数的调用在control。c 里面，大家可以看一下，只要把加速度和陀螺仪的相关数据，传进这个函数就可以了，就这么简单。
这个函数可以根据我们传递进去的传感器数据，计算表示姿态的四元数，计算出了四元数怎么用呢？请看程序末尾：
Q_ANGLE.Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.Rool = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
这两句话就是根据四元数，计算表示飞机姿态的欧拉角。
有了欧拉角，就可以方便的用于控制了。
听说四元数可以直接参与控制，我还不懂是怎么回事，有懂的大神还请解释解释啊，大家一起学习一下。