无名飞控姿态解算和控制

回到TIME.c进入Accel_Calibration_Check();//加速度标定检测

定义：

uint8_t flight_direction=6;uint8_t Accel_Calibration_Flag=0;//加速度计校准模式uint8_t Accel_Calibration_Finished[6]={0,0,0,0,0,0};//对应面校准完成标志位uint8_t Accel_Calibration_All_Finished=0;//6面校准全部校准完成标志位uint16_t Accel_Calibration_Makesure_Cnt=0;uint16_t Accel_flight_direction_cnt=0;void Accel_Calibration_Check(void){   uint16_t  i=0;   if(Throttle_Control==1000&&Yaw_Control>=80&&Roll_Control<=-40&&Pitch_Control>=40)   {      Accel_Calibration_Makesure_Cnt++;   }   if(Throttle_Control==1000      &&Yaw_Control>=80        &&Roll_Control<=-40          &&Pitch_Control>=40            &&Accel_Calibration_Makesure_Cnt>=200*3)//持续三秒  {      Bling_Mode=1;      Accel_Calibration_Flag=1;//加速度校准模式      Cal_Flag=0;      Bling_Set(&Light_1,1000,100,0.5,0,GPIOC,GPIO_Pin_4,1);      Bling_Set(&Light_2,1000,100,0.5,0,GPIOC,GPIO_Pin_5,1);      Bling_Set(&Light_3,1000,100,0.5,0,GPIOC,GPIO_Pin_10,1);      flight_direction=6;      Accel_Calibration_All_Finished=0;//全部校准完成标志位清零      Accel_Calibration_Makesure_Cnt=0;      for(i=0;i<6;i++)      {        Accel_Calibration_Finished[i]=0;//对应面标志位清零        acce_sample[i].x=0; //清空对应面的加速度计量        acce_sample[i].y=0; //清空对应面的加速度计量        acce_sample[i].z=0; //清空对应面的加速度计量      }      Page_Number=10;//OLED加速度计矫正页面      Reset_Mag_Calibartion(1);  }  if(Accel_Calibration_Flag==1)  {     if(Throttle_Control==1000&&Yaw_Control<=-80&&Roll_Control==0&&Pitch_Control==0)     {       Accel_flight_direction_cnt++;       if(Accel_flight_direction_cnt>=4*25)//100ms         flight_direction=0;     }     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control>=40&&Pitch_Control==0)     {       Accel_flight_direction_cnt++;       if(Accel_flight_direction_cnt>=4*25)//100ms         flight_direction=1;     }     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control<=-40&&Pitch_Control==0)     {       Accel_flight_direction_cnt++;       if(Accel_flight_direction_cnt>=4*25)//100ms         flight_direction=2;     }     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control==0&&Pitch_Control>=40)     {       Accel_flight_direction_cnt++;       if(Accel_flight_direction_cnt>=4*25)//100ms         flight_direction=3;     }     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control==0&&Pitch_Control<=-40)     {       Accel_flight_direction_cnt++;       if(Accel_flight_direction_cnt>=4*25)//100ms         flight_direction=4;     }     else if(Throttle_Control==1000&&Yaw_Control>80&&Roll_Control==0&&Pitch_Control==0)     {       Accel_flight_direction_cnt++;       if(Accel_flight_direction_cnt>=4*25)//100ms       flight_direction=5;     }     else     {       Accel_flight_direction_cnt/=2;     }   if(Accel_flight_direction_cnt>=200)  Accel_flight_direction_cnt=0; }}

整个过程可以看做有三个部分组成

第一个if判断用来计数

if(Throttle_Control==1000&&Yaw_Control>=80&&Roll_Control<=-40&&Pitch_Control>=40)    {       Accel_Calibration_Makesure_Cnt++;   }

当Accel_Calibration_Makesure_Cnt满足营情况时开始校准（Accel_Calibration_Makesure_Cnt>=250*3）持续三秒 ？？？？

当Accel_Calibration_Makesure_Cnt>=250*3成立时有：

if(Throttle_Control==1000      &&Yaw_Control>=80        &&Roll_Control<=-40          &&Pitch_Control>=40            &&Accel_Calibration_Makesure_Cnt>=250*3)//持续三秒  

校准赋初值

 Bling_Mode=1;      Accel_Calibration_Flag=1;//加速度校准模式      Cal_Flag=0;      Bling_Set(&Light_1,1000,100,0.5,0,GPIOC,GPIO_Pin_12,1);      Bling_Set(&Light_2,1000,100,0.5,0,GPIOC,GPIO_Pin_11,1);        Bling_Set(&Light_3,1000,100,0.5,0,GPIOC,GPIO_Pin_10,1);      flight_direction=6;      Accel_Calibration_All_Finished=0;//全部校准完成标志位清零      Accel_Calibration_Makesure_Cnt=0;      for(i=0;i<6;i++)      {        Accel_Calibration_Finished[i]=0;//对应面标志位清零        acce_sample[i].x=0; //清空对应面的加速度计量        acce_sample[i].y=0; //清空对应面的加速度计量        acce_sample[i].z=0; //清空对应面的加速度计量       }      Page_Number=10;//OLED加速度计矫正页面      Reset_Mag_Calibartion(1);

其中Bling_Set函数如下：

Bling_Light Light_1,Light_2,Light_3,Light_4;uint16_t Bling_Mode=0;void Bling_Set(Bling_Light *Light,               uint32_t Continue_time,//持续时间               uint16_t Period,//周期100ms~1000ms               float Percent,//0~100%               uint16_t  Cnt,               GPIO_TypeDef* Port,               uint16_t Pin               ,uint8_t Flag){  Light->Bling_Contiune_Time=Continue_time/4;//持续时间  Light->Bling_Period=Period;//周期  Light->Bling_Percent=Percent;//占空比  Light->Bling_Cnt=Cnt;  Light->Port=Port;//端口  Light->Pin=Pin;//引脚  Light->Endless_Flag=Flag;//无尽模式}

Bling_Light结构体定义：

typedef struct{  uint16_t Bling_Contiune_Time;//闪烁持续时间    uint16_t Bling_Period;//闪烁周期  float  Bling_Percent;//闪烁占空比  uint16_t  Bling_Cnt;//闪烁计数器  GPIO_TypeDef* Port; //端口  uint16_t Pin;//引脚  uint8_t Endless_Flag;//无尽模式}Bling_Light;

接着赋值

flight_direction=6;      Accel_Calibration_All_Finished=0;//全部校准完成标志位清零      Accel_Calibration_Makesure_Cnt=0;      for(i=0;i<6;i++)      {        Accel_Calibration_Finished[i]=0;//对应面标志位清零        acce_sample[i].x=0; //清空对应面的加速度计量        acce_sample[i].y=0; //清空对应面的加速度计量        acce_sample[i].z=0; //清空对应面的加速度计量       }      Page_Number=10;//OLED加速度计矫正页面

其中Acce_Unit acce_sample[6]={0};//三行6列，保存6面待矫正数据

typedef struct{ float x; float y; float z;}Acce_Unit;

上面是Acce_Unit定义

接着

Reset_Mag_Calibartion(1);

void Reset_Mag_Calibartion(uint8_t Type){  uint16 i=0;  for(i=0;i<12;i++)  {     Mag_360_Flag[0][i]=0;//清空采集角点    Mag_360_Flag[1][i]=0;//清空采集角点  }  Mag_Is_Okay_Flag[0]=0;  Mag_Is_Okay_Flag[1]=0;  Mag_Calibration_Mode=2;  if(Type==1)  Mag_Calibration_Flag=0; }  

加速度计校准开始：

  if(Accel_Calibration_Flag==1)  {     if(Throttle_Control==1000&&Yaw_Control<=-80&&Roll_Control==0&&Pitch_Control==0)      {          flight_direction=0;     }     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control>=40&&Pitch_Control==0)      {          flight_direction=1;     }      else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control<=-40&&Pitch_Control==0)      {          flight_direction=2;     }      else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control==0&&Pitch_Control>=40)      {          flight_direction=3;     }      else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control==0&&Pitch_Control<=-40)      {          flight_direction=4;     }     else if(Throttle_Control==1000&&Yaw_Control>80&&Roll_Control==0&&Pitch_Control==0)      {          flight_direction=5;     }   }

回到TIMC.c，进入Mag_Calibration_Check

void Mag_Calibration_Check(void){   uint16_t  i=0,j=0;   if(Throttle_Control==1000&&Yaw_Control>=80&&Roll_Control>=40&&Pitch_Control>=40) Mag_Calibration_Makesure_Cnt++;   if(Throttle_Control==1000      &&Yaw_Control>=80        &&Roll_Control>=40          &&Pitch_Control>=40           &&Mag_Calibration_Makesure_Cnt>250*4//持续4S             )//进入磁力计校准模式    {      Bling_Mode=2;      Mag_Calibration_Flag=1;//磁力计校准模式      Mag_Calibration_Mode=2;      Bling_Set(&Light_1,1000,500,0.2,0,GPIOC,GPIO_Pin_12,1);      Bling_Set(&Light_2,1000,500,0.5,0,GPIOC,GPIO_Pin_11,1);        Bling_Set(&Light_3,1000,500,0.7,0,GPIOC,GPIO_Pin_10,1);      Mag_Calibration_Makesure_Cnt=0;      Mag_Calibration_All_Finished=0;//全部校准完成标志位清零      for(i=0;i<2;i++)      {        Mag_Calibration_Finished[i]=0;//对应面标志位清零        for(j=0;j<12;j++) {Mag_360_Flag[i][j]=0;}      }      Page_Number=11;      Reset_Accel_Calibartion(1);  }    if(Mag_Calibration_Flag==1)  {     if(Throttle_Control==1000        &&Yaw_Control<=-80          &&Roll_Control==0            &&Pitch_Control==0) //第一面矫正     {            Mag_Calibration_Mode=0;         Mag_Is_Okay_Flag[1]=0;//单面数据采集完成标志位置0         for(i=0;i<12;i++) Mag_360_Flag[1][i]=0;//清空采集角遍历数据点     }  else if(Throttle_Control==1000             &&Yaw_Control>80               &&Roll_Control==0                 &&Pitch_Control==0) //第二面矫正     {            Mag_Calibration_Mode=1;         Mag_Is_Okay_Flag[1]=0;//单面数据采集完成标志位置0         for(i=0;i<12;i++) Mag_360_Flag[1][i]=0;//清空采集角遍历数据点     }    }  }

其中，调用了Reset_Accel_Calibartion

void Reset_Accel_Calibartion(uint8_t Type){  uint16 i=0;  for(i=0;i<6;i++)  {     Accel_Calibration_Finished[i]=0;//对应面标志位清零     acce_sample[i].x=0; //清空对应面的加速度计量     acce_sample[i].y=0; //清空对应面的加速度计量     acce_sample[i].z=0; //清空对应面的加速度计量   }  Accel_Calibration_All_Finished=0;//全部校准完成标志位清零    if(Type==1)  Accel_Calibration_Flag=0;    } 

uint8_t Mag_360_Flag[2][12]={0};

回到TIME.c,Bling_Working(Bling_Mode)，再校准的过程中量不同的灯

void Bling_Working(uint16 bling_mode){      if(bling_mode==0)//全灭      {          Bling_Process(&Light_1);          Bling_Process(&Light_2);          Bling_Process(&Light_3);      }      else if(bling_mode==1)//加速度计6面校准模式      {            if(flight_direction==0)//第一面校准准备            {              Bling_Process(&Light_1);              GPIO_SetBits(Light_2.Port,Light_2.Pin);              GPIO_SetBits(Light_3.Port,Light_3.Pin);            }            else if(flight_direction==1)//第二面校准准备            {              Bling_Process(&Light_2);              GPIO_SetBits(Light_1.Port,Light_1.Pin);              GPIO_SetBits(Light_3.Port,Light_3.Pin);            }            else if(flight_direction==2)//第三面校准准备            {              Bling_Process(&Light_1);              Bling_Process(&Light_2);              GPIO_SetBits(Light_3.Port,Light_3.Pin);            }            else if(flight_direction==3)//第四面校准准备            {              Bling_Process(&Light_3);              GPIO_SetBits(Light_1.Port,Light_1.Pin);              GPIO_SetBits(Light_2.Port,Light_2.Pin);            }            else if(flight_direction==4)//第五面校准准备            {              Bling_Process(&Light_1);              Bling_Process(&Light_3);              GPIO_SetBits(Light_2.Port,Light_2.Pin);            }            else if(flight_direction==5)//第六面校准准备            {              GPIO_SetBits(Light_1.Port,Light_1.Pin);              Bling_Process(&Light_2);              Bling_Process(&Light_3);            }            else            {              Bling_Process(&Light_1);              Bling_Process(&Light_2);              Bling_Process(&Light_3);            }      }      else if(bling_mode==2)//磁力计校准模式      {            if(Mag_Calibration_Mode==0)//水平面校准            {              Bling_Process(&Light_1);              Bling_Process(&Light_2);              GPIO_SetBits(Light_3.Port,Light_3.Pin);            }            else if(Mag_Calibration_Mode==1)////竖直平面校准            {              GPIO_SetBits(Light_1.Port,Light_1.Pin);              Bling_Process(&Light_2);              Bling_Process(&Light_3);            }            else            {              Bling_Process(&Light_1);              Bling_Process(&Light_2);              Bling_Process(&Light_3);            }}else if(bling_mode==3)//全灭{   GPIO_SetBits(GPIOC,GPIO_Pin_10);   GPIO_SetBits(GPIOC,GPIO_Pin_11);   GPIO_SetBits(GPIOC,GPIO_Pin_12);}   Bling_Process(&Light_4);}

void Bling_Process(Bling_Light *Light)//闪烁运行线程{  if(Light->Bling_Contiune_Time>=1)  Light->Bling_Contiune_Time--;  else GPIO_SetBits(Light->Port,Light->Pin);//置高,亮  if(Light->Bling_Contiune_Time!=0//总时间未清0     ||Light->Endless_Flag==1)//判断无尽模式是否开启  {    Light->Bling_Cnt++;    if(4*Light->Bling_Cnt>=Light->Bling_Period) Light->Bling_Cnt=0;//计满清零    if(4*Light->Bling_Cnt<=Light->Bling_Period*Light->Bling_Percent)       GPIO_ResetBits(Light->Port,Light->Pin);//置高，亮    else GPIO_SetBits(Light->Port,Light->Pin);//置低，灭  }}

 GPIO_SetBits(Light_2.Port,Light_2.Pin);设置指定的数据端口位

GPIOx：x可以是A，B，C，D或者E，来选择GPIO外设

GPIO_Pin：待设置的端口位该参数可以取GPIO_Pin_x(x可以是0-15)的任意组合

Light_1,GPIOC,GPIO_Pin_12
Light_2,GPIOC,GPIO_Pin_11
Light_3,GPIOC,GPIO_Pin_10

结束：Bling_Working( )

进入DMA_Send_StateMachine();//DMA传输

DMA_Vcan_Buff  Vcan_Buff;#define DMA_SEND_PERIOD 5//4*5=20ms,周期太小不易于观察波形void DMA_Send_StateMachine(void){  static uint16_t DMA_Send_Cnt=0;  DMA_Send_Cnt++;  if(DMA_Send_Cnt>=DMA_SEND_PERIOD)  {  Vcan_Buff.Head=0xfc030000;  Vcan_Buff.DataBuf[0]=NamelessQuad.Position[_YAW];  Vcan_Buff.DataBuf[1]=NamelessQuad.Speed[_YAW];  Vcan_Buff.DataBuf[2]=NamelessQuad.Acceleration[_YAW];;  Vcan_Buff.DataBuf[3]=Altitude_Estimate;  Vcan_Buff.DataBuf[4]=10*Pitch;  Vcan_Buff.DataBuf[5]=10*ACCE_X;  Vcan_Buff.DataBuf[6]=10*Roll;  Vcan_Buff.DataBuf[7]=10*ACCE_Y;  Vcan_Buff.End=0x000003fc;  Quad_DMA1_USART1_SEND((u32)(&Vcan_Buff),sizeof(Vcan_Buff));  DMA_Send_Cnt=0;  }}

其中Vcan_Buff是DMA_Vcan_Buff 类型的结构体;

typedef struct{  uint32_t Head;  float DataBuf[8];  uint32_t End;}DMA_Vcan_Buff;

另外函数Quad_DMA1_USART1_SEND

void    Quad_DMA1_USART1_SEND(u32 SendBuff,u16 len)//DMA---USART1传输{Quad_DMA_Config(DMA1_Channel4,(u32)&USART1->DR,(u32)SendBuff,len);//DMA1通道4,外设为串口1,存储器为SendBuff,长度len.USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);Quad_DMA_Enable(DMA1_Channel4);//while(DMA_GetFlagStatus(DMA1_FLAG_TC4) != SET);//DMA_ClearFlag(DMA1_FLAG_TC4);//清除发送完成标志}

首先是Quad_DMA_Config

DMA_InitTypeDef DMA_InitStructure;u16 DMA1_MEM_LEN;//保存DMA每次数据传送的长度/*DMA1的各通道配置这里的传输形式是固定的,这点要根据不同的情况来修改从存储器->外设模式/8位数据宽度/存储器增量模式DMA_CHx:DMA通道CHx      cpar:外设地址cmar:存储器地址         cndtr:数据传输量*/void Quad_DMA_Config(DMA_Channel_TypeDef* DMA_CHx,u32 cpar,u32 cmar,u16 cndtr){ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);//使能DMA传输        DMA_DeInit(DMA_CHx);   //将DMA的通道1寄存器重设为缺省值DMA1_MEM_LEN=cndtr;DMA_InitStructure.DMA_PeripheralBaseAddr = cpar;  //DMA外设ADC基地址DMA_InitStructure.DMA_MemoryBaseAddr =cmar;//DMA内存基地址DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;  //外设作为数据传输的目的地DMA_InitStructure.DMA_BufferSize = cndtr;  //DMA通道的DMA缓存的大小DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;  //外设地址寄存器不变DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;  //内存地址寄存器递增DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;  //数据宽度为8位DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //数据宽度为8位DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;  //工作在正常缓存模式DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; //DMA通道 x拥有中优先级DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;  //DMA通道x没有设置为内存到内存传输DMA_Init(DMA_CHx, &DMA_InitStructure);  //根据DMA_InitStruct中指定的参数初始化DMA的通道USART1_Tx_DMA_Channel所标识的寄存器}

具体是什么我也不知道，等看了32的资料再接着码吧！

TIME.c的最后一个函数TIM_ClearITPendingBit(TIM4,TIM_FLAG_Update);

清除TIM4的中断待处理位，

TIM_FLAG_Update，待检查的TIM中断待处理位