黑洞飞控代码分析（三）------姿态解算和控制

在收集需要的数据后，进行处理，解算姿态

在进入之前先看一看系统时钟的中断函数

/*
 * 函数名：SysTick_Handler
 * 描述  ：滴答中断函数
 * 输入  ：无
 * 输出  ：无
 */  
void SysTick_Handler(void)
{	
	static u16 cnt = 0;
	static u8 ms = 0;
	
	cnt++;

	Prepare_Data();  //准备数据
	if(Is_WFLY_Unlock())//判断是否解锁与是否在执行解锁动作	
	{		
		Is_WFLY_Lock();		//判断是否在执行上锁动作	
		IMU_Update();			//姿态更新频率为1KHz
		ms++;
		if(ms == 2)	 			//控制频率500Hz
		{ 
			ms = 0;			
			WFLY_PWM_Convert();
			Control(Angle,Exp_Angle);		
		}			
		if(cnt % 500 == 0)	
		{	
			LED_LEFT_BACK_TOGGLE;
			LED_RIGHT_BACK_TOGGLE;
			cnt = 0;
		}	
	}
	else//未解锁
	{
		Motor_PWM_Update(0,0,0,0);//确保安全
		if(cnt % 1000 == 0)	
		{	
			LED_LEFT_BACK_TOGGLE;
			LED_RIGHT_BACK_TOGGLE;
			cnt = 0;
		}	
	}

}

现在到了判断if(Is_WFLY_Unlock())//判断是否解锁与是否在执行解锁动作
怎么判断呢?

u8 unlock_flag = 0 ;					//解锁标志位，为0未解锁，为1解锁	
extern vs16 throttle;
extern S_FLOAT_XYZ Exp_Angle;
extern u16 Rc_Channel_Val[6];

/*
 * 函数名：Is_WFLY_Unlock
 * 描述  ：判断是否执行解锁动作
 * 输入  ：无
 * 输出  ：0：未解锁；1：解锁成功
 */  
u8 Is_WFLY_Unlock(void)
{
	static u16	cnt_unlock = 0;
	
	if( !unlock_flag )	//如果未解锁,只有在未解锁的清况才能解锁
	{
		if((Rc_Channel_Val[0]	<	1200) && (Rc_Channel_Val[1] < 1200) )			//左摇杆打到右下角
		{	
			if(((Rc_Channel_Val[2]	<	1200) && (Rc_Channel_Val[3]) > 1800) )//右摇杆打到左下角
			{       类似大疆的外八解锁	
				cnt_unlock++;
				if( (cnt_unlock % 20) == 0)								 //解锁过程中，每隔20ms闪烁一次
				{
					LED_LEFT_BACK_TOGGLE;//左边LED闪烁
					LED_RIGHT_BACK_TOGGLE;
				}
			}
		}
		else cnt_unlock = 0;													 //如果未解锁或解锁中断，则解锁时间计数清零
			
		if( cnt_unlock > 1000)												 //解锁动作持续1秒，则解锁
			{
				cnt_unlock = 0;
				SysTick->CTRL &= ~ SysTick_CTRL_ENABLE_Msk;//关闭中断
				AHRS_Date_Init(); 										 	 //每次解锁后都先初始化导航数据
				MPU6500_Date_Offset(5000);								 //校准陀螺仪
				SysTick->CTRL |=  SysTick_CTRL_ENABLE_Msk; //开启中断
				unlock_flag = 1;													 //成功解锁标志
				return 1;
			}	
		 else return 0;		
	}
	else return 1;//如果解锁了，则返回1
}

其中说一下

#define  LED_LEFT_BACK_TOGGLE   GPIOC->ODR ^= GPIO_Pin_13;//a与b的对应位进行异或运算
#define  LED_RIGHT_BACK_TOGGLE  GPIOB->ODR ^= GPIO_Pin_12;

而另外有定义

#define GPIO_Pin_12                ((uint16_t)0x1000)  /*!< Pin 12 selected */
#define GPIO_Pin_13                ((uint16_t)0x2000)  /*!< Pin 13 selected */

当掰杆计时达到1000以后，先关闭中断，先初始化导航数据

extern S_FLOAT_XYZ Exp_Angle;

/*
 * 函数名：AHRS_Date_Init
 * 描述  ：航姿数据初始化
 * 输入  ：无
 * 输出  ：无
 */ 
void AHRS_Date_Init(void)
{	
	int cnt;
	
	for(cnt	=0;cnt	<FILTER_LENGTH;cnt++)//FILTER_LENGTH=20

	{
		Acc_Buf[cnt].X = 0;
		Acc_Buf[cnt].Y = 0;
		Acc_Buf[cnt].Z = 0;
	}
  	
	q0 = 1;
	q1 = 0;
	q2 = 0;
	q3 = 0;
	
	q0_yaw = 1;
	q1_yaw = 0;
	q2_yaw = 0;
	q3_yaw = 0;
	
	Angle.X = 0;
	Angle.Y = 0;
	Angle.Z = 0;
	
	E