#include "include.h"

u8 hv_motor1_data[8];
u8 hv_motor2_data[8];
u8 hv_motor3_data[8];

void SysTick_Handler(void)
{
	sysTickUptime++;
}

void TIM1_UP_TIM10_IRQHandler(void)
{
  if(TIM_GetITStatus(TIM1, TIM_IT_Update))  //判断发生update事件中断
  {
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update); //清除update事件中断标志
		//放Dshot函数
		static u8 count = 0;
		if(count++>=4) count = 0;
		
		switch(count)
		{
			case 0:
			RThruster_DSHOT_DMA_Config();
			pwmWriteDigital(right_thruster.pid_data.dshot_data, right_thruster.pid_data.dshot_pwm, DMA1_Stream0);
			break;
			
			case 1:
			LThruster_DSHOT_DMA_Config();
			pwmWriteDigital(left_thruster.pid_data.dshot_data, left_thruster.pid_data.dshot_pwm, DMA1_Stream5);
			break;
			
			case 2:
			RTrack_DSHOT_DMA_Config();
			pwmWriteDigital(right_track_motor.pid_data.dshot_data, right_track_motor.pid_data.dshot_pwm, DMA1_Stream3);
			break;
			
			case 3:  
			LTrack_DSHOT_DMA_Config();
			pwmWriteDigital(left_track_motor.pid_data.dshot_data, left_track_motor.pid_data.dshot_pwm, DMA1_Stream5);
			break;
		
			default:break;
		}
	}
}

void ResearchProtection(MOTOR* motor, s16 aimSpeed)
{
	if((*motor).pid_data.last_aim<0 && aimSpeed>=0)
	{
		(*motor).pid_data.aim_speed += 2;
	}
	else
	{
		(*motor).pid_data.aim_speed = aimSpeed;
	}
	(*motor).pid_data.last_aim = (*motor).pid_data.aim_speed;
}

int maxcnt = 0;
void CAN1_RX0_IRQHandler(void)
{
	u32 id = 0;
	s16 value = 0;
	u8 RecData[8];
	u8 RecNum;
	u8 i=0;
	u8 j=0;
	RecNum = CAN1_Receive_Msg(RecData,&id);
	
	if(id == 0X014E570A)//4E59
	{
		if(RecData[1]==PIXHAWK_REQURE)
		{
			Pixhawk_requre = 13;
		}
		else if(RecData[1]==SET_PARME)
		{
			switch(RecData[0])
			{
				case TRACK_P: track_PID.P = Rec_Flash_Set(RecData);break;
				case TRACK_I: track_PID.I = Rec_Flash_Set(RecData);break;
				case TRACK_D: track_PID.D = Rec_Flash_Set(RecData);break;
				
				case LOWSPEED:   protect_parameters.low_speed_mag    = Rec_Flash_Set(RecData);break;
				case HIGHSPEED:  protect_parameters.high_speed_mag   = Rec_Flash_Set(RecData);break;
				case DIVIDI_RPM: protect_parameters.dividi_rpm   = Rec_Flash_Set(RecData);break;
				
				case MAXCURRENT: protect_parameters.max_current_lime = Rec_Flash_Set(RecData);break;
				case MAXTIME:    protect_parameters.time_lime = Rec_Flash_Set(RecData);break;
				
				case REST_STM32: 	SoftReset();break;
				
				case LEFT_TRACK: left_trank_direction = Rec_Flash_Set(RecData);break;
				case RIGHT_TRACK: right_trank_direction = Rec_Flash_Set(RecData);break;
//													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);
												  break;
				default:break;
			}
			Updata_requre = RecData[0];
		}
	}
	else if(id == 0X014E550A)
	{
		//分段数据包首帧
		if((RecData[RecNum-1]>>7)&1)
		{
			i = 2;
			CAN_RecData_i = 0;
			CAN_RecData_Toggle = 0;
			CAN_RecData_TID = RecData[RecNum-1]&0x1F;
		}
	  if(((RecData[RecNum-1]&0x1F) == CAN_RecData_TID) &&   //TransferID正确
			(((RecData[RecNum-1]>>5)&1) == CAN_RecData_Toggle) ) //反转位正确
	  {
			CAN_RecData_Toggle = (CAN_RecData_Toggle==0)?1:0;
			
			for(;i<RecNum-1;i++)
			{
				if(CAN_RecData_i>maxcnt) maxcnt = CAN_RecData_i;
				
				if(CAN_RecData_i > CAN_RecDataMax)
				{
					//出现错误,舍弃该包
					RecData[RecNum-1] = 0;
					CAN_RecData_i = 0;
					CAN_RecData_Toggle = 0;
					break;
				}
				CAN_RecData[CAN_RecData_i++] = RecData[i];
			}
		}
		//分段数据包尾帧
		if(((RecData[RecNum-1]>>6)&1)==1 && RecNum == 8)
		{
			if(CAN_RecData_i == CAN_RecDataMax) //成功接收全部数据,赋值
			{
				IWDG_Feed();
				//左履带      
				if(left_trank_direction == 0)
					value = (CAN_RecData[5]<<8|CAN_RecData[4]);
				else
					value = -1*(CAN_RecData[5]<<8|CAN_RecData[4]);
				//赋值
				if(left_track_motor.stal_data.stop_flag == 0)
				{
					ResearchProtection(&left_track_motor, value);
				}
				//右履带
				if(right_trank_direction == 0)
					value = (CAN_RecData[7]<<8|CAN_RecData[6]);
				else 
					value = -1*(CAN_RecData[7]<<8|CAN_RecData[6]);
				//赋值
				if(right_track_motor.stal_data.stop_flag == 0)
				{
					ResearchProtection(&right_track_motor, value);
				}
				//2#
				value = CAN_RecData[3]<<8|CAN_RecData[2];
				if(value<=1000 && value>=-1000 && left_thruster.stal_data.stop_flag == 0)	//motor2.pid_data.aim_speed = value;
				{
					ResearchProtection(&left_thruster, value);
					left_thruster.pid_data.dshot_pwm = GetDShotValue(left_thruster.pid_data.aim_speed, left_thruster.twin_data.speed_lim);
				}
				//1#
				value = CAN_RecData[1]<<8|CAN_RecData[0];
				if(value<=1000 && value>=-1000 && right_thruster.stal_data.stop_flag == 0)//	motor1.pid_data.aim_speed = value;
				{
					ResearchProtection(&right_thruster, value);
					right_thruster.pid_data.dshot_pwm = GetDShotValue(right_thruster.pid_data.aim_speed, right_thruster.twin_data.speed_lim);
				}
				light_pwm = (CAN_RecData[9]<<8)|CAN_RecData[8];//灯 PWM
				usbl_opendown = (CAN_RecData[11]<<8)|CAN_RecData[10];//usbl	
				//照明灯启动
				//light_pwm = 1;
			}
			CAN_RecData_i = 0;
			CAN_RecData_Toggle = 0;
		}
	}
	/*
	转发比弗迪电调信息
	else if(id == 0x014E600B)
	{
		for(j=0; j<8; j++)
		{
			hv_motor1_data[j] = RecData[j];
		}
	}
	else if(id == 0x014E610C)
	{
		for(j=0; j<8; j++)
		{
			hv_motor2_data[j] = RecData[j];
		}
	}
	else if(id == 0x014E620D)
	{
		for(j=0; j<8; j++)
		{
			hv_motor3_data[j] = RecData[j];
		}
	}
	*/
}

//左履带
void USART2_IRQHandler(void)
{
	if(USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET)
	{
		GetMotorData(USART2, &left_track_motor);
		USART_ClearITPendingBit(USART2, USART_IT_RXNE);
	}
	else if(USART_GetITStatus(USART2,USART_IT_IDLE)!=RESET)
	{
		USART2->SR;
		USART2->DR;
		AnaMotorData(&left_track_motor);
	}
}

//左推进器
void USART3_IRQHandler(void)
{
	if(USART_GetITStatus(USART3,USART_IT_RXNE)!=RESET)
	{
		GetMotorData(USART3, &left_thruster);
		USART_ClearITPendingBit(USART3, USART_IT_RXNE);
	}
	else if(USART_GetITStatus(USART3,USART_IT_IDLE)!=RESET)
	{
		USART3->SR;
		USART3->DR;
		AnaMotorData(&left_thruster);
	}
}

//右推进器
void UART5_IRQHandler(void)
{
	if(USART_GetITStatus(UART5,USART_IT_RXNE)!=RESET)
	{
		GetMotorData(UART5, &right_thruster);
		USART_ClearITPendingBit(UART5, USART_IT_RXNE);
	}
	else if(USART_GetITStatus(UART5,USART_IT_IDLE)!=RESET)
	{
		UART5->SR;
		UART5->DR;
		AnaMotorData(&right_thruster);
	}
}

//右履带
void USART6_IRQHandler(void)
{
	if(USART_GetITStatus(USART6,USART_IT_RXNE)!=RESET)
	{
		GetMotorData(USART6, &right_track_motor);
		USART_ClearITPendingBit(USART6, USART_IT_RXNE);
	}
	else if(USART_GetITStatus(USART6,USART_IT_IDLE)!=RESET)
	{
		USART6->SR;
		USART6->DR;
		AnaMotorData(&right_track_motor);
	}
}

void DMA1_Stream0_IRQHandler(void)
{
	if(DMA_GetITStatus(DMA1_Stream0, DMA_IT_TCIF0))  //判断DMA传输完成中断  
	{
		DMA_ClearITPendingBit(DMA1_Stream0, DMA_IT_TCIF0);
		DMA_Cmd(DMA1_Stream0, DISABLE);
	}
}

void DMA1_Stream3_IRQHandler(void)
{
	if(DMA_GetITStatus(DMA1_Stream3, DMA_IT_TCIF3))  //判断DMA传输完成中断  
	{
		DMA_ClearITPendingBit(DMA1_Stream3, DMA_IT_TCIF3);
		DMA_Cmd(DMA1_Stream3, DISABLE);
	}
}

void DMA1_Stream5_IRQHandler(void)
{
	if(DMA_GetITStatus(DMA1_Stream5, DMA_IT_TCIF5))  //判断DMA传输完成中断  
	{
		DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5);
		DMA_Cmd(DMA1_Stream5, DISABLE);
	}
}


void DMA2_Stream0_IRQHandler(void)
{
	static u16 AVG_i=0;
	static u16 CurrentBuffPtr[6];
	
	if(DMA_GetITStatus(DMA2_Stream0, DMA_IT_TCIF0))  //判断DMA传输完成中断  
	{
		DMA_ClearITPendingBit(DMA2_Stream0, DMA_IT_TCIF0);
		//DMA_Cmd(DMA2_Stream0, ENABLE); //使能DMA,连续模式下不需要该使能
		//数据转移程序
		if(DMA_GetCurrentMemoryTarget(DMA2_Stream0) == DMA_Memory_0)
		{
			CurrentBuffPtr[0] = uAD_Buff_A[0];
			CurrentBuffPtr[1] = uAD_Buff_A[1];
			CurrentBuffPtr[2] = uAD_Buff_A[2];
			CurrentBuffPtr[3] = uAD_Buff_A[3];
			CurrentBuffPtr[4] = uAD_Buff_A[4];
			CurrentBuffPtr[5] = uAD_Buff_A[5];
		}
		else
		{
			CurrentBuffPtr[0] = uAD_Buff_B[0];
			CurrentBuffPtr[1] = uAD_Buff_B[1];
			CurrentBuffPtr[2] = uAD_Buff_B[2];
			CurrentBuffPtr[3] = uAD_Buff_B[3];
			CurrentBuffPtr[4] = uAD_Buff_B[4];
			CurrentBuffPtr[5] = uAD_Buff_B[5];
		}
		
		AVG_i++;
		
		//CurrentBuffPtr[0] = 4095;
		
		left_thruster.twin_data.avg_sum += CurrentBuffPtr[1];//0
		right_thruster.twin_data.avg_sum += CurrentBuffPtr[3];//1
		left_track_motor.twin_data.avg_sum += CurrentBuffPtr[0];//2
		right_track_motor.twin_data.avg_sum += CurrentBuffPtr[2];//3
		
		Voltage_sum += CurrentBuffPtr[4];
		
		NTC_ADC_VALUE_SUM += CurrentBuffPtr[5];
		
		if(AVG_i >= 400)
		{
			DMA400Finishi = 1;
			AVG_i = 0;
			
			left_thruster.twin_data.current = (float)left_thruster.twin_data.avg_sum*0.00004f;
			right_thruster.twin_data.current = (float)right_thruster.twin_data.avg_sum*0.00004f;
			left_track_motor.twin_data.current = (float)left_track_motor.twin_data.avg_sum*0.00004f;
			right_track_motor.twin_data.current = (float)right_track_motor.twin_data.avg_sum*0.00004f;
			
			left_thruster.twin_data.avg_sum = 0;
			right_thruster.twin_data.avg_sum = 0;
			left_track_motor.twin_data.avg_sum = 0;
			right_track_motor.twin_data.avg_sum = 0;
			
			Voltage = ((float)Voltage_sum)*3.27856/102400;
			Voltage_sum = 0;
			
			temperature = Get_Temperature(NTC_ADC_VALUE_SUM/400);
			NTC_ADC_VALUE_SUM = 0;

		}
	}
}




void HardFault_Handler(void)
{
  /* Go to infinite loop when Hard Fault exception occurs */
  while (1)
  {
  }
}