#define  __GLOBALS__
#include "include.h"

int main(void)
{
	//电机结构体初始化
	All_motor_Config();
	//中断优先组分配
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_3);
	//时钟初始化
	cycleCounterInit();
	//读取flash数据
	Flash_ReadInf();
	//Dshot输出初始化
	Dshot_Config();
	//为初始化电机初始化延时
	//ad采集电流初始化
	ADC_Config();
	//adc采集校准初始化
	//Motor_AD_correct();
	//CAN通讯初始化
	CAN_Config();
	//串口初始化
	USART_Config();
	//开关量初始化
	LED_Config();
	//USBL_Config();
	//Light_Config();
	Overcurrent_Detection_Config();
	
	delay_ms(3000);
	//看门狗初始化
	IWDG_Config(4,1000);
	
	while(1)
	{
		//Motor_Stall_check();//堵转检测
		
		SysTick_1ms_Task();
		
		SysTick_10ms_Task();
		
		SysTick_20ms_Task();
		
		SysTick_100ms_Task();
		
		SysTick_500ms_Task();
		
		Other_Task();
	}
}

u32 cntLight = 0;
u32 cntUSBL = 0;
u32 cntVIDEO = 0;
void SysTick_1ms_Task()//1000hz
{
	static u32 last_sysTickUptime = 0;
	static u32 cntADC = 0;
	static u32 adc_value=0;
	
	if((sysTickUptime - last_sysTickUptime) >= 1)
	{
		last_sysTickUptime = sysTickUptime;
		
		//灯过流保护
		if(!Read_LIGHT)
		{
			if(++cntLight>2)
			{
				CUT_LIGHT;
				light_err = 10;
				light_pwm = 0;
			}
		}
		else
		{
			cntLight = 0;
			
		}
		
		//USBL过流保护
		if(!Read_USBL)
		{
			if(++cntUSBL>2)
			{
				CUT_USBL;
				usbl_err = 1;
			}
		}
		else
		{
			cntUSBL = 0;
		}
		
		//摄像头过流保护
		if(!Read_VIDEO)
		{
			if(++cntVIDEO>2)
			{
				CUT_VIDEO;
			}
		}
		else
		{
			cntVIDEO = 0;
		}
		
		if(++cntADC<=10)
		{
			adc_value += Get_SOFT_ADC(ADC1, ADC_Channel_14);
		}
		else
		{
			cntADC = 0;
			humidity = adc_value/10;
			
			if(humidity > 1500)
			{
				humidity_err = 1;
			}
			else
			{
				humidity_err = 0;
			}
			adc_value = 0;
		}
	}
}


void SysTick_10ms_Task()//100hz
{
	static u32 last_sysTickUptime = 0;
	
	if((sysTickUptime - last_sysTickUptime) >= 10)
	{
		last_sysTickUptime = sysTickUptime;
		//任务列表
		//1.发送参数反馈
		Send_Pixhawk_requre();
		//2.
		
	}
}

void SysTick_20ms_Task()//50hz
{
	static u32 last_sysTickUptime = 0;
	
	if((sysTickUptime - last_sysTickUptime) >= 20)
	{
		last_sysTickUptime = sysTickUptime;
		//任务列表
		//1.左右履带PID控制
		Motor_Control(&left_track_motor, &track_PID, 0);
		Motor_Control(&right_track_motor, &track_PID, 0);
		//2.
	}
}

void SysTick_100ms_Task()//10hz
{
	static u32 last_sysTickUptime = 0;
	
	if((sysTickUptime - last_sysTickUptime) >= 20)//100
	{
		last_sysTickUptime = sysTickUptime;
		//任务列表
		//1.发送履带推进器反馈数据
		static u8 send_pixhawk_cnt = 0;
		
		if(send_pixhawk_cnt++ >= 3) send_pixhawk_cnt = 0;
		
		switch(send_pixhawk_cnt)
		{
			case 0:
				Send_CAN_DATA1();
				Send_CAN_DATA2();
				break;
			case 1:
				Send_CAN_DATA3();
				Send_CAN_DATA4();
			break;
			case 2:
				Send_CAN_DATA5();
				Send_CAN_DATA6();
			break;
			/*
			case 3:
				Send_CAN_MOTOR1DATA();
				Send_CAN_MOTOR2DATA();
				Send_CAN_MOTOR3DATA();
			*/
		default:break;
		}
	}
}


void SysTick_500ms_Task()//2hz
{
	static u32 last_sysTickUptime = 0;
	
	if((sysTickUptime - last_sysTickUptime) >= 500)
	{
		last_sysTickUptime = sysTickUptime;
		//任务列表
		//1.闪烁led
		LED_TURN;
		
		//测试默认开
//		light_pwm = 1;
//		usbl_opendown = 1;

		if(light_pwm && light_err == 0)
			LIGHT_ON;
		else
			LIGHT_OFF;
			
		//USBL启动
		if(usbl_opendown && usbl_err == 0)
			USBL_ON;
		else
			USBL_OFF;
	}
}

void Other_Task(void)
{
	//1.反馈flash
	if(Updata_requre > 0)
	{
		Send_Updata_requre();
	}
}


void Overcurrent_Detection_Config(void)
{
	//过流检测
	GPIO_IN_Config(RCC_AHB1Periph_GPIOA, GPIOA, GPIO_Pin_2);
	GPIO_IN_Config(RCC_AHB1Periph_GPIOB, GPIOB, GPIO_Pin_8|GPIO_Pin_9);
	
	//通断控制
	GPIO_Config(RCC_AHB1Periph_GPIOA, GPIOA, GPIO_Pin_1|GPIO_Pin_15);
	GPIO_Config(RCC_AHB1Periph_GPIOF, GPIOF, GPIO_Pin_15);
	
	//相机默认开
	GPIO_SetBits(GPIOA, GPIO_Pin_1);
	
	//灯,USBL默认关
	GPIO_ResetBits(GPIOA, GPIO_Pin_15);
	GPIO_ResetBits(GPIOF, GPIO_Pin_15);
	
	//测试 默认开
	//GPIO_SetBits(GPIOA, GPIO_Pin_15);
	//GPIO_SetBits(GPIOF, GPIO_Pin_15);
}

void Stall_check(MOTOR *Motor)// void(*PWMoutput)(TIM_TypeDef*,uint32_t)
{
	float Magnification = 2;
	
	if((*Motor).usart_data.rpm >= protect_parameters.dividi_rpm)
	{
		Magnification = protect_parameters.high_speed_mag;
	}
	else
	{
		Magnification = protect_parameters.low_speed_mag;
	}
	
	if((*Motor).numble < 9 && (*Motor).stal_data.stop_flag == 0)
	{
		//1.推进器缠绕保护
		if(((*Motor).twin_data.current - (*Motor).twin_data.current_correct)
				> ((*Motor).twin_data.STcurrent*Magnification + 0.5f))
		{
			++(*Motor).twin_data.twin_cnt;
		}
		else
		{
			(*Motor).twin_data.twin_cnt = 0;
		}
		
		//出现缠绕,削弱输出
		if((*Motor).twin_data.twin_cnt >= 80)//2S  1000
		{
			(*Motor).twin_data.current = 0;
			(*Motor).twin_data.twin_cnt = 0;
			if((*Motor).twin_data.speed_lim>0)
			{
				(*Motor).twin_data.speed_lim -= 1;
				if((*Motor).twin_data.speed_lim<1)
				{
					(*Motor).twin_data.speed_lim = 0;
				}
			}
			else
			{
				(*Motor).twin_data.speed_lim = 0;
			}
		}
		//3.启动保护 3s无法旋转判定启动堵塞
//		if(((*Motor).pid_data.aim_speed>200 || (*Motor).pid_data.aim_speed<-200) && (*Motor).usart_data.rpm<20)
//		{
//			(*Motor).stal_data.unormal_cnt++;
//			if((*Motor).stal_data.unormal_cnt>=120) (*Motor).stal_data.stop_flag = -1;			
//		}
//		else
//		{
//			if((*Motor).stal_data.unormal_cnt>0) (*Motor).stal_data.unormal_cnt--;
//		}
		
		//2.推进器瞬时过流保护 25ms采样值超过设定值(STALL_Param2.MaxCurrent)
		if((*Motor).twin_data.current >= protect_parameters.max_current_lime)
		{
			(*Motor).stal_data.stop_flag = -1;//停转至重启
		}
	}
	else if((*Motor).numble >= 9 && (*Motor).stal_data.stop_flag == 0)
	{
		//减速电机保护
		//1.履带电机过功率保护
		if((*Motor).twin_data.current * ((*Motor).usart_data.voltage) > 700)
		{
			//过功率时间累计 以及 防止计数溢出
			if(++(*Motor).stal_data.stall_cnt>=65535) 
				(*Motor).stal_data.stall_cnt = (int16_t)(protect_parameters.time_lime/25) + 1;
		}
		//2.履带瞬时过流保护 25ms采样值超过设定值(STALL_Param2.MaxCurrent)
		if((*Motor).twin_data.current >= protect_parameters.max_current_lime-5)
		{
			(*Motor).stal_data.stop_flag = 120;//-1;//停转至重启
			Motor_err_flag |= 1<<((*Motor).numble-1);
		}
	}
	
	//打嗝自恢复中
	if((*Motor).stal_data.stop_flag != 0) 
	{
		if((*Motor).stal_data.stop_flag > 0)
		{
			(*Motor).stal_data.stop_flag--;
			
			if((*Motor).stal_data.stop_flag == 0)
			{
				Motor_err_flag &= ~(1<<((*Motor).numble-1));
				(*Motor).stal_data.stall_err = 0;
			}
		}
		else if((*Motor).stal_data.stop_flag<0)
		{
			(*Motor).stal_data.stall_err = 1;
			Motor_err_flag |= (*Motor).stal_data.stall_err<<((*Motor).numble-1);
		}
		//停转
		(*Motor).pid_data.aim_speed = 0;
		(*Motor).pid_data.dshot_pwm = 0;
		(*Motor).pid_data.pwm =0;
	}
}

void Motor_Stall_check(void)
{
	if(DMA400Finishi == 1)// 堵转保护 25ms
	{ 
		DMA400Finishi = 0;
		
		//get_run_time();
		
		Stall_check(&left_thruster);
		Stall_check(&right_thruster);
		Stall_check(&left_track_motor);
		Stall_check(&right_track_motor);
	}
}


void Single_Motor_Config(MOTOR *motor,u8 num)
{
	(*motor).numble = num;
	
	(*motor).pid_data.aim_speed = 0;
	
	(*motor).twin_data.speed_lim = 10;
	(*motor).twin_data.block_err = 0;
	(*motor).twin_data.block_cnt = 0;
	(*motor).twin_data.twin_cnt  = 0;
	(*motor).twin_data.current_correct = 0;
	
	(*motor).stal_data.overload = 2;
	(*motor).stal_data.stall_cnt = 0;
	(*motor).stal_data.stop_flag = 0;
	(*motor).stal_data.stall_err = 0;
}

void All_motor_Config(void)
{
	track_PID.P = 0.6;
	track_PID.I = 0.3;
	track_PID.D = 0;
	
	protect_parameters.max_current_lime = 15;
	protect_parameters.time_lime        = 300;
	protect_parameters.low_speed_mag    = 2.5;
	protect_parameters.high_speed_mag   = 3.5;
	protect_parameters.dividi_rpm       = 70;
	
	Single_Motor_Config(&left_thruster,1);
	Single_Motor_Config(&right_thruster,2);
	
	Single_Motor_Config(&left_track_motor,9);
	Single_Motor_Config(&right_track_motor,10);
	//履带方向
	left_trank_direction = 1;
	right_trank_direction = 1;
}

void LED_Config(void)
{
	GPIO_Config(RCC_AHB1Periph_GPIOF, GPIOF, GPIO_Pin_0);
}

void USBL_Config(void)
{ 
	GPIO_Config(RCC_AHB1Periph_GPIOA, GPIOA, GPIO_Pin_15);
}

void Light_Config(void)
{
	GPIO_Config(RCC_AHB1Periph_GPIOF, GPIOF, GPIO_Pin_15);
}

void CAN_Config(void)
{
	CAN_GPIO_Config(RCC_AHB1Periph_GPIOA, GPIOA, GPIO_Pin_11, GPIO_PinSource11, GPIO_AF_CAN1);
	CAN_GPIO_Config(RCC_AHB1Periph_GPIOA, GPIOA, GPIO_Pin_12, GPIO_PinSource12, GPIO_AF_CAN1);
	
	CAN_NVIC_Config(CAN1_RX0_IRQn, 6, 1);
	
	CAN_Mod_Config(RCC_APB1Periph_CAN1, CAN1, 0X014E550A, 0X014E570A);//0X014E580A, 0X014E590A);
}

void TIM_Config(void)
{
	TIM_Mod_Config(RCC_APB2PeriphClockCmd, RCC_APB2Periph_TIM1, TIM1, 84-1, 400-1, 1, 0);
	TIM_NVIC_Config(TIM1_UP_TIM10_IRQn, 1, 0);
}

void ADC_Config(void)
{
	//u8 ADC_Channel[ADC_CHANNEL] = {ADC_Channel_7, ADC_Channel_8, ADC_Channel_14, ADC_Channel_15, ADC_Channel_12, ADC_Channel_13};
	u8 ADC_Channel[ADC_CHANNEL] = {ADC_Channel_12, ADC_Channel_13};
	
	//电流
	ADC_GPIO_Config(RCC_AHB1Periph_GPIOF, GPIOF, GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_9|GPIO_Pin_10);
	
	//电压
	ADC_GPIO_Config(RCC_AHB1Periph_GPIOC, GPIOC, GPIO_Pin_2);
	
	//温度
	ADC_GPIO_Config(RCC_AHB1Periph_GPIOC, GPIOC, GPIO_Pin_3);
	
	//漏水检测
	ADC_GPIO_Config(RCC_AHB1Periph_GPIOC, GPIOC, GPIO_Pin_4);
	
	ADC_SOFT_Mode_Config(RCC_APB2Periph_ADC1, ADC1, 1);
	
	ADC_DMA_Mode_Config(RCC_APB2Periph_ADC3, ADC_ExternalTrigConv_T5_CC1, ADC3, ADC_Channel, ADC_CHANNEL);
	
	ADC_NVIC_Config(DMA2_Stream0_IRQn, 0, 0);
	
	ADC_DMA_Config(RCC_AHB1Periph_DMA2, DMA_Channel_2, ADC3, DMA2_Stream0);
	
	ADC_TIM_Config(TIM5, 84-1, 20,  RCC_APB1PeriphClockCmd, RCC_APB1Periph_TIM5, TIM_OC1Init, TIM_OC1PreloadConfig);
}

void Dshot_Config(void)
{
	//PA5
	Dshot_GPIO_Config(RCC_AHB1Periph_GPIOA, GPIOA, GPIO_Pin_5, GPIO_PinSource5, GPIO_AF_TIM2);
	Dshot_TIM_Config(RCC_APB1PeriphClockCmd, RCC_APB1Periph_TIM2, TIM2, 4-1, 70-1, TIM_OC1Init, TIM_OC1PreloadConfig);
	Dshot_NVIC_Config(DMA1_Stream5_IRQn, 0, 0);
	//PA7
	Dshot_GPIO_Config(RCC_AHB1Periph_GPIOA, GPIOA, GPIO_Pin_7, GPIO_PinSource7, GPIO_AF_TIM3);
	Dshot_TIM_Config(RCC_APB1PeriphClockCmd, RCC_APB1Periph_TIM3, TIM3, 4-1, 70-1, TIM_OC2Init, TIM_OC2PreloadConfig);
	Dshot_NVIC_Config(DMA1_Stream5_IRQn, 0, 0);
	//PB7
	Dshot_GPIO_Config(RCC_AHB1Periph_GPIOB, GPIOB, GPIO_Pin_7, GPIO_PinSource7, GPIO_AF_TIM4);
	Dshot_TIM_Config(RCC_APB1PeriphClockCmd, RCC_APB1Periph_TIM4, TIM4, 4-1, 70-1, TIM_OC2Init, TIM_OC2PreloadConfig);
	Dshot_NVIC_Config(DMA1_Stream3_IRQn, 0, 0);
	//PB6
	Dshot_GPIO_Config(RCC_AHB1Periph_GPIOB, GPIOB, GPIO_Pin_6, GPIO_PinSource6, GPIO_AF_TIM4);
	Dshot_TIM_Config(RCC_APB1PeriphClockCmd, RCC_APB1Periph_TIM4, TIM4, 4-1,70-1, TIM_OC1Init, TIM_OC1PreloadConfig);
	Dshot_NVIC_Config(DMA1_Stream0_IRQn, 0, 0);
	//定时器初始化
	TIM_Mod_Config(RCC_APB2PeriphClockCmd, RCC_APB2Periph_TIM1, TIM1, 84-1, 400-1, 1, 0);

	TIM_NVIC_Config(TIM1_UP_TIM10_IRQn, 1, 0);
}

void Flash_WriteInf()
{
	__disable_irq();
	STMFLASH_Write(FLASH_ADDR1(FLASH_INIT_FLAG),&Init_Flash_Data,1);
	IWDG_Feed();
	STMFLASH_Write(FLASH_ADDR1(TRACK_P_ADDR),(u32*)&track_PID.P,1);
	STMFLASH_Write(FLASH_ADDR1(TRACK_I_ADDR),(u32*)&track_PID.I,1);
	STMFLASH_Write(FLASH_ADDR1(TRACK_D_ADDR),(u32*)&track_PID.D,1);	
	IWDG_Feed();
	STMFLASH_Write(FLASH_ADDR1(STALL_LMag),(u32*)&protect_parameters.low_speed_mag,1);
	STMFLASH_Write(FLASH_ADDR1(STALL_HMag),(u32*)&protect_parameters.high_speed_mag,1);
	STMFLASH_Write(FLASH_ADDR1(STALL_RPM), (u32*)&protect_parameters.dividi_rpm, 1);
	IWDG_Feed();
	STMFLASH_Write(FLASH_ADDR1(STALL_MaxCurrent),(u32*)&protect_parameters.max_current_lime,1);
	STMFLASH_Write(FLASH_ADDR1(STALL_Time), (u32*)&protect_parameters.time_lime, 1);
	IWDG_Feed();
	STMFLASH_Write(FLASH_ADDR1(LEFT_TRACK_DIR),(u32*)&left_trank_direction,1);
	STMFLASH_Write(FLASH_ADDR1(RIGHT_TRACK_DIR), (u32*)&right_trank_direction, 1);
	__enable_irq();
}

void Flash_ReadInf()
{ 
	if(STMFLASH_ReadWord(FLASH_ADDR1(FLASH_INIT_FLAG))!=0x12345678)//该单片机未写入数据
	{
		Flash_WriteInf();//初始化数据
	}
	
	track_PID.P = ReadFloatWord(FLASH_ADDR1(TRACK_P_ADDR));
	track_PID.I = ReadFloatWord(FLASH_ADDR1(TRACK_I_ADDR));
	track_PID.D = ReadFloatWord(FLASH_ADDR1(TRACK_D_ADDR));
	
	protect_parameters.low_speed_mag   = ReadFloatWord(FLASH_ADDR1(STALL_LMag));
	protect_parameters.high_speed_mag  = ReadFloatWord(FLASH_ADDR1(STALL_HMag));
	protect_parameters.dividi_rpm  = ReadFloatWord(FLASH_ADDR1(STALL_RPM));
	
	protect_parameters.max_current_lime = ReadFloatWord(FLASH_ADDR1(STALL_MaxCurrent));
	protect_parameters.time_lime        = ReadFloatWord(FLASH_ADDR1(STALL_Time));
	
	left_trank_direction = STMFLASH_ReadWord(FLASH_ADDR1(LEFT_TRACK_DIR));
	right_trank_direction = STMFLASH_ReadWord(FLASH_ADDR1(RIGHT_TRACK_DIR));
}

void Flash_ResetInf(void)
{
	Flash_WriteInf();
	Flash_ReadInf();
}

//void Single_AD_correct(MOTOR *Motor)
//{
//	(*Motor).twin_data.current_correct += (*Motor).twin_data.current*0.01;
//}
//void Motor_AD_correct(void)
//{
//	u8 i = 0;
//	while(i<100)
//	{ 
//		if(DMA400Finishi == 1)
//		{
//			i++;
//			DMA400Finishi = 0;
//			Single_AD_correct(&motorRT);
//			Single_AD_correct(&motorLT);
//			Single_AD_correct(&motorRB);
//			Single_AD_correct(&motorLB);
//		}
//	}
//}

void USART_Config(void)
{
	//PA3  usart2
	USART_GPIO_Config(RCC_AHB1Periph_GPIOA, GPIO_AF_USART2, GPIOA, GPIO_Pin_3, GPIO_PinSource3);
	USART_NVIC_Config(USART2_IRQn, 3, 0);
	USART_MOD_Config(RCC_APB1PeriphClockCmd, RCC_APB1Periph_USART2, USART2, 115200);
	//PB11 usart3
	USART_GPIO_Config(RCC_AHB1Periph_GPIOB, GPIO_AF_USART3, GPIOB, GPIO_Pin_11, GPIO_PinSource11);
	USART_NVIC_Config(USART3_IRQn, 3, 0);
	USART_MOD_Config(RCC_APB1PeriphClockCmd, RCC_APB1Periph_USART3, USART3, 115200);
	//PD2  uart5
	USART_GPIO_Config(RCC_AHB1Periph_GPIOD, GPIO_AF_UART5, GPIOD, GPIO_Pin_2, GPIO_PinSource2);
	USART_NVIC_Config(UART5_IRQn, 3, 0);
	USART_MOD_Config(RCC_APB1PeriphClockCmd, RCC_APB1Periph_UART5, UART5, 115200);
	//PG9  usart6
	USART_GPIO_Config(RCC_AHB1Periph_GPIOG, GPIO_AF_USART6, GPIOG, GPIO_Pin_9, GPIO_PinSource9);
	USART_NVIC_Config(USART6_IRQn, 3, 0);
	USART_MOD_Config(RCC_APB2PeriphClockCmd, RCC_APB2Periph_USART6, USART6, 115200);
}

float A = 0.072811;
float B = -0.006079;
float C = 0.000193;
void Equation(MOTOR* Motor)
{
	(*Motor).twin_data.STcurrent = A + B*((*Motor).usart_data.rpm) + C*((*Motor).usart_data.rpm)*((*Motor).usart_data.rpm);
}

uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
{
	uint8_t crc_u, i;
	crc_u = crc;
	crc_u ^= crc_seed;
	for ( i=0; i<8; i++)
	{
		crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
	}
	return (crc_u);
}

uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
{
	uint8_t crc = 0, i;
	for( i=0; i<BufLen; i++)
	{
		crc = update_crc8(Buf[i], crc);
	}
	return (crc);
}

void GetMotorData(USART_TypeDef* USARTx, MOTOR *Motor)
{
	u8 byte = USART_ReceiveData(USARTx);
	if((*Motor).usart_data.buff_cnt<10)
		(*Motor).usart_data.buff[(*Motor).usart_data.buff_cnt] = byte;
	(*Motor).usart_data.buff_cnt++;
}

void AnaMotorData(MOTOR *Motor)
{
	if((*Motor).usart_data.buff_cnt == 10)
	{	
		if(get_crc8((*Motor).usart_data.buff,9) == (*Motor).usart_data.buff[9])//若数据有效
		{ 
			(*Motor).usart_data.temperature = (*Motor).usart_data.buff[0];
			(*Motor).usart_data.voltage     = (float)((*Motor).usart_data.buff[1]<<8
																							 |(*Motor).usart_data.buff[2])/100;
			(*Motor).usart_data.consumption = (*Motor).usart_data.buff[5]<<8
																			 |(*Motor).usart_data.buff[6];
			(*Motor).usart_data.rpm         = ((*Motor).usart_data.buff[7]<<8
																				|(*Motor).usart_data.buff[8]);//*100/7
			
			if((*Motor).numble ==9 || (*Motor).numble == 10) (*Motor).usart_data.rpm = (*Motor).usart_data.rpm*0.743;

			Equation(Motor);
		}
	}
	(*Motor).usart_data.buff_cnt = 0;
}

u16 GetDShotValue(int16_t value, int8_t lime)
{
	u16 DShotValue = 0;
	
	//lime = 10;//关闭缠绕限功率
	
	if(value >= 0)
	{
		DShotValue = (int16_t)(value*lime*0.1047);
		if(DShotValue > 942) DShotValue = 942;
	}
	else
	{
		DShotValue = 1047 - (int16_t)(value*lime*0.1);//1047 - (-300*0.001*1*1000)
		if(DShotValue > 1900) DShotValue = 1900;
	}
	
	if(lime == 0) DShotValue = 0;
	
	return DShotValue;
}


void Send_Pixhawk_requre(void)
{
	switch(Pixhawk_requre)
	{
		case 4:Send_Flash_Set(TRACK_P,RESPOND,track_PID.P);break;
		case 5:Send_Flash_Set(TRACK_I,RESPOND,track_PID.I);break;
		case 6:Send_Flash_Set(TRACK_D,RESPOND,track_PID.D);break;
		case 7:Send_Flash_Set(LOWSPEED,RESPOND,protect_parameters.low_speed_mag);break;
		case 8:Send_Flash_Set(HIGHSPEED,RESPOND,protect_parameters.high_speed_mag);break;
		case 9:Send_Flash_Set(DIVIDI_RPM,RESPOND,protect_parameters.dividi_rpm);break;
		case 10:Send_Flash_Set(MAXCURRENT,RESPOND,protect_parameters.max_current_lime);break;
		case 11:Send_Flash_Set(MAXTIME,RESPOND,protect_parameters.time_lime);break;
		case 12:Send_Flash_Set(LEFT_TRACK,RESPOND,left_trank_direction);break;
		case 13:Send_Flash_Set(RIGHT_TRACK,RESPOND,right_trank_direction);break;
		default:break;
	}
	if(Pixhawk_requre>0) Pixhawk_requre--;
}

void Send_Updata_requre(void)
{
	Flash_WriteInf();
	
	switch (Updata_requre)
	{
		case TRACK_P:
			Send_Flash_Set(TRACK_P,RESPOND,track_PID.P);break;
		case TRACK_I:
			Send_Flash_Set(TRACK_I,RESPOND,track_PID.I);break;
		case TRACK_D:
			Send_Flash_Set(TRACK_D,RESPOND,track_PID.D);break;
		case LOWSPEED:
			Send_Flash_Set(LOWSPEED,RESPOND,protect_parameters.low_speed_mag);break;
		case HIGHSPEED:
			Send_Flash_Set(HIGHSPEED,RESPOND,protect_parameters.high_speed_mag);break;
		case DIVIDI_RPM:
			Send_Flash_Set(DIVIDI_RPM,RESPOND,protect_parameters.dividi_rpm);break;
		case MAXCURRENT:
			Send_Flash_Set(MAXCURRENT,RESPOND,protect_parameters.max_current_lime);break;
		case MAXTIME:
			Send_Flash_Set(MAXTIME,RESPOND,protect_parameters.time_lime);break;
		case LEFT_TRACK:
			Send_Flash_Set(LEFT_TRACK,RESPOND,left_trank_direction);break;
		case RIGHT_TRACK:
			Send_Flash_Set(RIGHT_TRACK,RESPOND,right_trank_direction);break;
		default:break;
	}
	Updata_requre = 0;
}

void Motor_Control(MOTOR* Motor,PID_PARAMETERS* MotorPID, u8 usePID)
{
	int16_t Add      = 0;//PID增量
	//int8_t  Iseparat = 1;//积分分离标志位
	
	if(usePID) //使用PID
	{
		(*Motor).pid_data.new_speed = (*Motor).usart_data.rpm;//获取转速 

		//若当前目标速度与上次的目标速度的方向不一致(即需要反转)
		if((*Motor).pid_data.aim_speed*(*Motor).pid_data.last_aim<0 && (*Motor).pid_data.new_speed>0) 
		{
			(*Motor).pid_data.aim_speed = 0;  
			(*Motor).pid_data.err = 0;        
			(*Motor).pid_data.last_err = 0;   
			(*Motor).pid_data.last_aim = 0;   
		}	                                                            
		else                                                          
			(*Motor).pid_data.last_aim = (*Motor).pid_data.aim_speed;   

		//手动将测速反转
		if((*Motor).pid_data.aim_speed>0) (*Motor).pid_data.err = (*Motor).pid_data.aim_speed - (*Motor).pid_data.new_speed;
		else							   (*Motor).pid_data.err = (*Motor).pid_data.aim_speed + (*Motor).pid_data.new_speed;

		//if((*Motor).pid.Err > 50) Iseparat = 0;
		//else                      Iseparat = 1;

		//增量式PID + 积分分离 
		Add = (*MotorPID).P*((*Motor).pid_data.err - (*Motor).pid_data.last_err)  +
				(*MotorPID).I* (*Motor).pid_data.err + 
				(*MotorPID).D*((*Motor).pid_data.err - (*Motor).pid_data.last_err*2 + (*Motor).pid_data.pret_err);

		(*Motor).pid_data.pwm += Add;

		//防反转 若aim为正则不输出负PWM,反之亦然
		if((*Motor).pid_data.aim_speed>=0)  (*Motor).pid_data.pwm = max((*Motor).pid_data.pwm,0);
		else                 		        (*Motor).pid_data.pwm = min((*Motor).pid_data.pwm,0);

		//死区
		if((*Motor).pid_data.aim_speed<20 && (*Motor).pid_data.aim_speed > -20) //&& (*Motor).pid_data.new_speed < 30
		{
			(*Motor).pid_data.pwm = 0;
			(*Motor).pid_data.err = 0;
			(*Motor).pid_data.last_err = 0;
			(*Motor).pid_data.pret_err = 0;
		}

		//限幅
		(*Motor).pid_data.pwm = min((*Motor).pid_data.pwm, 1047);
		(*Motor).pid_data.pwm = max((*Motor).pid_data.pwm,-1000);

		if((*Motor).pid_data.pwm < 0) (*Motor).pid_data.dshot_pwm = 1047 - (*Motor).pid_data.pwm;
		else                          (*Motor).pid_data.dshot_pwm = (*Motor).pid_data.pwm;

		(*Motor).pid_data.pret_err  = (*Motor).pid_data.last_err;
		(*Motor).pid_data.last_err  = (*Motor).pid_data.err;
	}
	else //不使用PID
	{
		//limit Dshort by speed
		if((*Motor).pid_data.aim_speed >= 0)
		{
			u16 Dshort_limit = (u16)(((u32)3845*(*Motor).usart_data.rpm +(u32)1867200)/10000);
			
			if(Dshort_limit>850 ) Dshort_limit = 850;

			else if(Dshort_limit<320) Dshort_limit = 320;
			
			(*Motor).pid_data.dshot_limit = Dshort_limit;
			
			(*Motor).pid_data.dshot_pwm = (*Motor).pid_data.aim_speed;
		//	if((*Motor).pid_data.dshot_pwm>=1047) (*Motor).pid_data.dshot_pwm = 1047;
			if((*Motor).pid_data.dshot_pwm>=Dshort_limit) (*Motor).pid_data.dshot_pwm = Dshort_limit;
	  }
		else
		{
			u16 Dshort_limit = (u16)(((u32)3832*(*Motor).usart_data.rpm +(u32)1377600)/10000);//1377600
			
			if(Dshort_limit>800) 			Dshort_limit = 800;
				
			else if(Dshort_limit<280) Dshort_limit = 280;
			
			(*Motor).pid_data.dshot_limit = Dshort_limit;
			
			(*Motor).pid_data.dshot_pwm = 1048 - (*Motor).pid_data.aim_speed;
			//if((*Motor).pid_data.dshot_pwm>=(1048))  (*Motor).pid_data.dshot_pwm = 1048;
			
			if((*Motor).pid_data.dshot_pwm>=(1048+Dshort_limit))  (*Motor).pid_data.dshot_pwm = 1048+Dshort_limit;
		}
	}
}

void Motor_AD_correct(void)
{
	u8 i = 0;
	while(i<100)
	{
		if(DMA400Finishi == 1)
		{
			i++;
			DMA400Finishi = 0;
			
			left_thruster.twin_data.current_correct += left_thruster.twin_data.current*0.01;
			right_thruster.twin_data.current_correct += right_thruster.twin_data.current*0.01;
			left_track_motor.twin_data.current_correct += left_track_motor.twin_data.current*0.01;
			right_track_motor.twin_data.current_correct += right_track_motor.twin_data.current*0.01;
		}
	}
}


void SoftReset(void)
{
  __set_FAULTMASK(1); // 关闭所有中断
  NVIC_SystemReset(); // 复位
}