#include "stm32f4xx.h"                  // Device header
#include "stm32f4xx_conf.h"
#include "Visual.h"
#include "UART.h"
#include "gimbal.h"
#include "AttitudeAlgorithms.h"
#include"Delay.h"

float Visual_Yaw,Visual_Pitch,Visual_Roll,Visual_Delay;//视觉数据偏航角,视觉数据俯仰角
uint8_t Visual_RxHEXPacket[4],Visual_ReceiveFlag;//视觉数据接收缓冲区,视觉数据接收完成标志位

/*
 *函数简介:视觉初始化
 *参数说明:无
 发送至下位机数据
|数据 1|数据 2|数据 3|数据 4|数据 5|
|----|----|----|----|----|
|0x09|0x14|yaw 轴|Pitch 轴|0x18|

 *返回类型:无
 *备注:初始化UART2(USART1)
 Freq=Sys_APB1TIM/(PSC+1)/(ARR+1)=84MHz/(PSC+1)/(ARR+1)
 */
void Visual_Init(void)
{/*
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE);//开启时钟
	
	TIM_InternalClockConfig(TIM12);//选择时基单元的时钟(TIM9)
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;//配置时基单元（配置参数）
	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;//配置时钟分频为1分频
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;//配置计数器模式为向上计数
	TIM_TimeBaseInitStructure.TIM_Period=500-1;//配置自动重装值ARR
	TIM_TimeBaseInitStructure.TIM_Prescaler=840-1;//配置分频值PSC,定时50ms
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter=0;//配置重复计数单元的置为0
	TIM_TimeBaseInit(TIM12,&TIM_TimeBaseInitStructure);//初始化TIM12
	
	TIM_ClearFlag(TIM12,TIM_FLAG_Update);//清除配置时基单元产生的中断标志位
	
	TIM_ITConfig(TIM12,TIM_IT_Update,ENABLE);//使能更新中断
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//选择NVIC分组
	
	NVIC_InitTypeDef NVIC_InitStructure1;//配置NVIC（配置参数）
	NVIC_InitStructure1.NVIC_IRQChannel=TIM8_BRK_TIM12_IRQn;//选择中断通道为TIM12
	NVIC_InitStructure1.NVIC_IRQChannelCmd=ENABLE;//使能中断通道
	NVIC_InitStructure1.NVIC_IRQChannelPreemptionPriority=3;//TIM12的抢占优先级
	NVIC_InitStructure1.NVIC_IRQChannelSubPriority=3;//TIM12的响应优先级
	NVIC_Init(&NVIC_InitStructure1);//初始化NVIC
	TIM_Cmd(TIM12,ENABLE);//启动定时器
	*/
	Visual_SendData();

}
void Visual_SendData(void)
{
	SendData MCUMessage;
	MCUMessage.curr_yaw=AttitudeAlgorithms_DegYaw;
	MCUMessage.curr_pitch=AttitudeAlgorithms_DegPitch;
	MCUMessage.state=(uint8_t)('a');
	MCUMessage.mark=(uint8_t)(0);
	MCUMessage.anti_top=(uint8_t)(0);
	MCUMessage.enemy_color=(uint8_t)(1);
	MCUMessage.delta_x=(uint8_t)(1);
	MCUMessage.delta_y=(uint8_t)(1);
	UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));//UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));
	/*
	UART2_SendByte((float)AttitudeAlgorithms_DegYaw);
	UART2_SendByte((float)AttitudeAlgorithms_DegRoll);
	//UART2_SendByte((float)AttitudeAlgorithms_DegPitch);
	
	UART2_SendByte((uint8_t)0);
	UART2_SendByte((uint8_t)'a');
	UART2_SendByte((uint8_t)0);
	
	UART2_SendByte((uint8_t)0);
	UART2_SendByte((uint8_t)1);
	UART2_SendByte((uint8_t)1);
*/
	//UART2_Printf("%f %f a 1 1 1 1 1 ",AttitudeAlgorithms_DegYaw,AttitudeAlgorithms_DegRoll);

	UART2_SendByte((uint8_t)'\n');

	
	

//UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));
}
/*   buff[0] = 's';
    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
    buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
    buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
    buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
    buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
    buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
    buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
    buff[9] = 'e';
*/

/*
 *函数简介:UART2串口中断接收视觉数据
 *参数说明:无
 *返回类型:无
 *备注:无
 */
void USART1_IRQHandler(void)
{
	#define DataLength		8//有效数据位数
	char Visual_RxHEXPacket[10];

	static uint8_t UART2_RxData;
	//char Message[10];
	static int RxHEXState=0;//定义静态变量用于接收模式的选择
	static int pRxHEXState=0;//定义静态变量用于充当计数器
	char startflag;//包头1
	//char endflag;//包头2

	if(USART_GetITStatus(USART1,USART_IT_RXNE)==SET)//查询接收中断标志位
	{
		UART2_RxData=USART_ReceiveData(USART1);//将数据存入缓存区
		startflag=(char)UART2_RxData;


		if(RxHEXState==0)//模式0-等待包头1(0x09)
		{

			if( startflag =='s')//检测包头
				RxHEXState=1;//转入模式1
		}
		else if(RxHEXState==1)//模式1-接收有效数据
		{
			pRxHEXState=0;//复位计数器
			Visual_RxHEXPacket[pRxHEXState]=UART2_RxData;//接收数据
			pRxHEXState++;
			
			if(pRxHEXState>=DataLength)
				RxHEXState=2;//转入模式2
		}
		else if(RxHEXState==2)//模式2-等待包尾
		{
			if(UART2_RxData=='e')//检测包尾
			{
				Visual_Yaw=(float)((uint8_t)Visual_RxHEXPacket[0]<<8 | Visual_RxHEXPacket[1]);//右摇杆右左
		        Visual_Pitch=(float)((uint8_t)Visual_RxHEXPacket[2]<<8 | Visual_RxHEXPacket[3]);//右摇杆上下
		        Visual_Roll=(float)((uint8_t)Visual_RxHEXPacket[4]<<8 | Visual_RxHEXPacket[5]);//左摇杆右左
		        Visual_Delay=(float)((uint8_t)Visual_RxHEXPacket[6]<<8 | Visual_RxHEXPacket[7]);//左摇杆上下
				//Visual_Yaw=(int16_t)((uint16_t)Visual_RxHEXPacket[0]<<8 | Visual_RxHEXPacket[1])*0.01f;
				//Visual_Pitch=(int16_t)((uint16_t)Visual_RxHEXPacket[2]<<8 | Visual_RxHEXPacket[3])*0.01f;
				Visual_ReceiveFlag=1;
			}
			RxHEXState=0;//回到模式0
		}
	}
	
	USART_ClearITPendingBit(USART1,USART_IT_RXNE);//清除接收中断标志位
	Visual_SendData();
}