无名飞控姿态解算和控制

于 2017-12-17 21:36:20 发布
阅读量1.1k 收藏 8
点赞数 1
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/wang2012113132/article/details/78828180
版权

回到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中断待处理位



























JRlipton
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值