#include "stm32f4xx.h"                  // Device header
#include "stm32f4xx_conf.h"
#include <stdio.h>
#include "RM_C.h"
#include "AttitudeAlgorithms.h"
#include "Gimbal.h"
#include "visual.h"
#include "UART.h"

#define DELAY_TIME 5   //D1定时的计数值


unsigned int Delay_Count;   //D1的延时初始值
unsigned char Delay_Flag;   //D1延时时间到标志位

int main(void)
{
	Warming_Init();//报警初始化
	LED_BON();//蓝灯点亮表示代码在运行
	Delay_s(1);//等待车身稳定以便于陀螺仪消除零偏
	AttitudeAlgorithms_Init();//姿态解算初始化
	ZeroOffset_Calibration();//零偏校准
	Delay_s(1);//延时,等待校准和模块启动
	LinkCheck_Init();//连接检测初始化
	RefereeSystem_Init();//图传链路初始化
	Visual_Init();//视觉初始化
	CloseLoopControl_Init();//闭环控制初始化
	Remote_Init();//遥控器初始化
	//UART2_Init();
	//UART2_SendInit();
	//UART2_ReceiveInit();
	Delay_Count = DELAY_TIME;  //给延时计数赋值
		UART2_Printf("ok\n");

	while(1)
	{
		IWDG_ReloadCounter();//喂狗

		//Visual_Trans();//视觉数据传输
		
		CToC_MasterSendControl();//CToC发送遥控器控制数据
		CToC_MasterSendData();//CToC发送遥控器摇杆数据
		
		
		Visual_SendData();


		if(Delay_Count)   //每次大循环只执行一次    
		{
			Delay_Count --;    //只对计数值减一次1，就跳出if
			if(!Delay_Count) Delay_Flag = 1;   //定时时间到
		}
		
		
		if(Delay_Flag)   //定时时间到就执行一定的操作
		{
			Delay_Flag = 0;  //定时到标志位清零
			//Debug();
			Visual_SendData();
			Visual_Trans();//视觉数据传输
			Delay_Count = DELAY_TIME;   //计数值重装，为下一次非阻塞延时做准备
		}
		


		
		Delay_us(1500);//CToC周期1.5ms
	}
}