/*
 * main implementation: use this 'C' sample to create your own application
 *
 */


#include "derivative.h" /* include peripheral declarations SSKEAZ128M4 */

volatile  uint32_t	SecurityUnlocked_timer = 0;
volatile  uint32_t	SessionNotDefault_timer = 0;
volatile  uint32_t	io_in_process_timer = 0;
volatile  uint32_t	adc_process_timer = 0;
volatile  uint32_t	printf_process_timer = 0;
volatile  uint32_t	reset_timer = 0;
volatile  uint32_t	cantx_timeout_timer = 0;
volatile  uint32_t	lin_process_timer = 0;
volatile  uint32_t 	onlylisten_timer = TIMER_SLAVE_ONLYLISTEN;
volatile  uint32_t	hall_not_detect_timer[TOTAL_MOTOR_NUM][TOTAL_WAY_NUM] = {{HARD_STUCK_TIME,HARD_STUCK_TIME},{HARD_STUCK_TIME,HARD_STUCK_TIME}};
volatile  uint32_t	curr_not_detect_timer[TOTAL_MOTOR_NUM] = {HARD_STUCK_TIME,HARD_STUCK_TIME};
volatile  uint32_t	motor_slide_timer[TOTAL_MOTOR_NUM] = {0};
volatile  uint32_t  motor_start_delay_timer = 0;
volatile  uint32_t	anti_clip_reverse_timer[TOTAL_MOTOR_NUM] = {0};
volatile  uint32_t	t61_off_timer = 0;
volatile  uint32_t	led_blink_timer = 0;
uint8_t	motor_slide_flag[TOTAL_MOTOR_NUM] = {0};
uint8_t OTA_HW_Ver[HW_VER_SIZE+1]={0};
uint8_t OTA_SN[SN_SIZE+1]={0};
uint8_t OTA_FW_Ver[FW_VER_SIZE+1]={0};

uint32_t adc_current1_sum = 0;
uint32_t adc_current2_sum = 0;
uint32_t adc_bat_sum = 0;
uint16_t sum_count=0;

uint16_t adc_current1 = 0;
uint16_t adc_current2 = 0;
uint16_t adc_bat = 0;
uint8_t adc_bat_error_flag=0;

uint8_t u8WriteDataBuff[SAVE_BYTE_SIZE]={0};
uint8_t enter_low_power_flag=POWER_STATE_ONLYLISTEN;

uint16_t led_r=0x0000;
uint16_t led_g=0x0000;
uint16_t led_b=0x0000;

uint16_t led_r_target=0x0000;
uint16_t led_g_target=0x0000;
uint16_t led_b_target=0x0000;

uint8_t led_blink_loop=0;

void ICS_Init_main(void);
void GPIO_Init_main(void);
void GPIO_power_off(void);
void UART0_Init_main(void);
void UART1_LIN_Init_main(void);
void UART2_Init_main(void);
void FTM_Init_main(void);
void RTC_Init_main(void);
void CAN_Init_main(void);
void CRC_Init_main(void);
void ADC_Init_main(void);
void Flash_Init_main(void);
void KBI_Init_main(void);
void PCM_Init_main(void);
void uart_commend_read(void);
void lvw_det(void);

void ADC_CallBack(void);
void adc_read_function(void);
void enter_low_power_mode(void);
void KBI1_Task(void);
void FTM0_Task(void);
void FTM1_Task(void);
void FTM2_Task(void);
void JumpToUserApplication( unsigned int userSP,  unsigned int userStartup);

int main(void)
{
	ICS_Init_main();
	GPIO_Init_main();
	UART0_Init_main();
	UART1_LIN_Init_main();
	UART2_Init_main();
	Flash_Init_main();
	FTM_Init_main();
	//RTC_Init_main();
	CRC_Init_main();
	ADC_Init_main();
	PCM_Init_main();
	KBI_Init_main();
	CAN_Init();

	//1.0.1 HW V0B IO pin change
	memcpy(OTA_HW_Ver,"BOSS_BTN_AGS",sizeof("BOSS_BTN_AGS"));
	memcpy(OTA_SN,"20241018",SN_SIZE);
	memcpy(OTA_FW_Ver,"00000101",FW_VER_SIZE);
	printf("%s , %s , %s \r\n",OTA_HW_Ver,OTA_SN,OTA_FW_Ver);

	while(1)
	{
		if(io_in_process_timer==0)
		{
			io_in_detect_process();//IO_DETECT_CYCLE_TIME = 5ms
			io_in_process_timer=IO_DETECT_CYCLE_TIME;
		}
		if(printf_process_timer==0)
		{
			//printf("M1_pos=%04X  M2_pos=%04X\r\n",motor_current_position[DEF_MOTOR1],motor_current_position[DEF_MOTOR2]);
			static uint16_t pos_pre[7]={0};
			if(pos_pre[0]!=motor_current_position[0]||
				pos_pre[1]!=motor_current_position[1]||
				pos_pre[2]!=AGS_CurrentPos_Status_LIN1||
				pos_pre[3]!=AGS_CurrentPos_Status_LIN2||
				pos_pre[4]!=led_r||
				pos_pre[5]!=led_g||
				pos_pre[6]!=led_b)
			{
				printf("M1_pos=%04X M2_pos=%04X  AGS1_pos=%02X AGS2_pos=%02X  AGS1_err=%02X AGS2_err=%02X  LED(%d,%d,%d) onlylisten_timer=%ld\r\n",
						motor_current_position[DEF_MOTOR1],motor_current_position[DEF_MOTOR2],
						AGS_CurrentPos_Status_LIN1,AGS_CurrentPos_Status_LIN2,
						lin1_error_flag,lin2_error_flag,
						led_r/LED_SLOW_SPEED,led_g/LED_SLOW_SPEED,led_b/LED_SLOW_SPEED,onlylisten_timer);
			}
			pos_pre[0]=motor_current_position[0];
			pos_pre[1]=motor_current_position[1];
			pos_pre[2]=AGS_CurrentPos_Status_LIN1;
			pos_pre[3]=AGS_CurrentPos_Status_LIN2;
			pos_pre[4]=led_r;
			pos_pre[5]=led_g;
			pos_pre[6]=led_b;
			printf_process_timer=50;
		}
		if(adc_process_timer==0)
		{
			adc_read_function();
			adc_process_timer=8;//8ms
		}
		if(led_signal==0x10 || led_signal==0x20 || t61_off_timer>0)
		{
			//printf("led_signal=0x%02X t61_off_timer=%ld\r\n",led_signal,t61_off_timer);
			onlylisten_timer = TIMER_SLAVE_ONLYLISTEN;
		}
		if(enter_low_power_flag==POWER_STATE_LOWPOWER)
		{
			lin_sleep();
			lin_sleep_finish=1;
		}
		else
		{
			lin_process();
			lin_sleep_finish=0;
		}
		if(enter_low_power_flag==POWER_STATE_LOWPOWER && lin_sleep_finish==1)
		{
			enter_low_power_mode();
		}
		CAN_Receive();
		CAN_TP_data_send();
		//lvw_det();
	}

	return 0;
}

void ICS_Init_main(void)
{
	ICS_ConfigType ICS_set={0};		/* Declaration of ICS_setup structure */
	ICS_set.u8ClkMode=ICS_CLK_MODE_FEI; /* ICS in FLL engaged internal mode*/
	ICS_set.bdiv=0;
	ICS_Init(&ICS_set);             /*Initialization of core clock at 48MHz, bus clock 24 MHz*/
}

void GPIO_Init_main(void)
{
	/* Power*/
	gpio_output_set(	POWER_12V_EN2_PORT,	POWER_12V_EN2_PIN,	1);
	CONFIG_PIN_AS_GPIO(	POWER_12V_EN2_PORT,	POWER_12V_EN2_PIN,	OUTPUT);
	gpio_output_set(	POWER_12V_EN1_PORT,	POWER_12V_EN1_PIN,	1);
	CONFIG_PIN_AS_GPIO(	POWER_12V_EN1_PORT,	POWER_12V_EN1_PIN,	OUTPUT);
	gpio_output_set(	POWER_5V_EN_PORT,	POWER_5V_EN_PIN,	1);
	CONFIG_PIN_AS_GPIO(	POWER_5V_EN_PORT,	POWER_5V_EN_PIN,	OUTPUT);

	/* Relay */
	gpio_output_set(	MOTOR1_P_EN_PORT,	MOTOR1_P_EN_PIN,	0);
	gpio_output_set(	MOTOR1_N_EN_PORT,	MOTOR1_N_EN_PIN,	0);
	gpio_output_set(	MOTOR2_P_EN_PORT,	MOTOR2_P_EN_PIN,	0);
	gpio_output_set(	MOTOR2_N_EN_PORT,	MOTOR2_N_EN_PIN,	0);
	gpio_output_set(	EXT_RY_EN_PORT,		EXT_RY_EN_PIN,		0);
	CONFIG_PIN_AS_GPIO(	MOTOR1_P_EN_PORT,	MOTOR1_P_EN_PIN,	OUTPUT);
	CONFIG_PIN_AS_GPIO(	MOTOR1_N_EN_PORT,	MOTOR1_N_EN_PIN,	OUTPUT);
	CONFIG_PIN_AS_GPIO(	MOTOR2_P_EN_PORT,	MOTOR2_P_EN_PIN,	OUTPUT);
	CONFIG_PIN_AS_GPIO(	MOTOR2_N_EN_PORT,	MOTOR2_N_EN_PIN,	OUTPUT);
	CONFIG_PIN_AS_GPIO(	EXT_RY_EN_PORT,		EXT_RY_EN_PIN,		OUTPUT);

	/* CAN EN */
	gpio_output_set(	CAN_STB_PORT,		CAN_STB_PIN,		0);		/* CAN ENABLE = CAN_STB LOW*/
	CONFIG_PIN_AS_GPIO(	CAN_STB_PORT,		CAN_STB_PIN,		OUTPUT);

	/* LIN EN */
	gpio_output_set(	LIN_EN_PORT,		LIN_EN_PIN,			1);		/* LIN ENABLE = LIN_EN HIGH*/
	CONFIG_PIN_AS_GPIO(	LIN_EN_PORT,		LIN_EN_PIN,			OUTPUT);

	/* INPUT */
	CONFIG_PIN_AS_GPIO(	IO1_IN_PORT,	IO1_IN_PIN,	INPUT);
	CONFIG_PIN_AS_GPIO(	IO2_IN_PORT,	IO2_IN_PIN,	INPUT);
	CONFIG_PIN_AS_GPIO(	IO3_IN_PORT,	IO3_IN_PIN,	INPUT);
	CONFIG_PIN_AS_GPIO(	IO4_IN_PORT,	IO4_IN_PIN,	INPUT);
	CONFIG_PIN_AS_GPIO(	IO5_IN_PORT,	IO5_IN_PIN,	INPUT);
	ENABLE_INPUT(		IO1_IN_PORT,	IO1_IN_PIN	);
	ENABLE_INPUT(		IO2_IN_PORT,	IO2_IN_PIN	);
	ENABLE_INPUT(		IO3_IN_PORT,	IO3_IN_PIN	);
	ENABLE_INPUT(		IO4_IN_PORT,	IO4_IN_PIN	);
	ENABLE_INPUT(		IO5_IN_PORT,	IO5_IN_PIN	);//2024/10/18 IO2_IN_PIN to IO5_IN_PIN

	CONFIG_PIN_AS_GPIO(	MOTOR1_HALL_PORT,	MOTOR1_HALL_PIN,	INPUT);
	CONFIG_PIN_AS_GPIO(	MOTOR2_HALL_PORT,	MOTOR2_HALL_PIN,	INPUT);
	ENABLE_INPUT(		MOTOR1_HALL_PORT,	MOTOR1_HALL_PIN	);
	ENABLE_INPUT(		MOTOR2_HALL_PORT,	MOTOR2_HALL_PIN	);
}

void GPIO_power_off(void)
{
	/* Power*/
	gpio_output_set(	POWER_12V_EN2_PORT,	POWER_12V_EN2_PIN,	0);
	gpio_output_set(	POWER_12V_EN1_PORT,	POWER_12V_EN1_PIN,	0);
	gpio_output_set(	POWER_5V_EN_PORT,	POWER_5V_EN_PIN,	0);

	/* Relay */
	gpio_output_set(	MOTOR1_P_EN_PORT,	MOTOR1_P_EN_PIN,	0);
	gpio_output_set(	MOTOR1_N_EN_PORT,	MOTOR1_N_EN_PIN,	0);
	gpio_output_set(	MOTOR2_P_EN_PORT,	MOTOR2_P_EN_PIN,	0);
	gpio_output_set(	MOTOR2_N_EN_PORT,	MOTOR2_N_EN_PIN,	0);
	gpio_output_set(	EXT_RY_EN_PORT,		EXT_RY_EN_PIN,		0);

	/* CAN EN */
	gpio_output_set(	CAN_STB_PORT,		CAN_STB_PIN,		1);		/* CAN DISABLE = CAN_STB HIGH*/

	/* LIN EN */
	gpio_output_set(	LIN_EN_PORT,		LIN_EN_PIN,			0);		/* LIN DISABLE = LIN_EN LOW*/
}

void UART1_LIN_Init_main(void)
{
	UART_ConfigType Uart1_Config={{0}};
	SIM_PINSEL1 |= SIM_PINSEL1_UART1PS_MASK; /* SIM_PINSEL1 bit 12 set 1,UART1 TX PTF3,RX PTF2 */
	Uart1_Config.sctrl1settings.bits.bM=0; 		/* 8 bit Mode */
	Uart1_Config.sctrl1settings.bits.bPe=0; 	/* Parity disable */
	Uart1_Config.bSbns=0;						/* One stop bit*/
	Uart1_Config.sctrl2settings.bits.bRe=1;		/* Receiver enable*/
	Uart1_Config.sctrl2settings.bits.bTe=1;		/* Transmitter enable*/
	Uart1_Config.sctrl2settings.bits.bRie=1; 	/*!< Receiver Interrupt Enable for RDRF*/
	Uart1_Config.sctrl2settings.bits.bTcie=0; 	/*!< Transmission Complete Interrupt Enable for TC*/
	Uart1_Config.sctrl2settings.bits.bTie=0;		/* Transmit buffer empty interrupt enable*/
	Uart1_Config.sctrl2settings.bits.bIlie=0;
	Uart1_Config.sctrl2settings.bits.bSbk=0;
	Uart1_Config.sctrl2settings.bits.bRwu=0;
	Uart1_Config.u32SysClkHz = 24000000;   		/* Bus clock in Hz*/
	Uart1_Config.u32Baudrate = 19200;     		/* Baud rate*/
	UART_SetCallback(UART1,&UART1_Isr);
	UART_Init(UART1,&Uart1_Config);
	UART1->S2 |= UART_S2_BRK13_MASK;
}

void UART0_Init_main(void)
{
	UART_ConfigType Uart0_Config={{0}};
	SIM_PINSEL0 &= (~SIM_PINSEL_UART0PS_MASK); /* SIM_PINSEL0 bit 7 set 0,UART0 TX PTB1,RX PTB0 */
	Uart0_Config.sctrl1settings.bits.bM=0; 		/* 8 bit Mode */
	Uart0_Config.sctrl1settings.bits.bPe=0; 		/* Parity disable */
	Uart0_Config.bSbns=0;						/* One stop bit*/
	Uart0_Config.sctrl2settings.bits.bRe=1;		/* Receiver enable*/
	Uart0_Config.sctrl2settings.bits.bTe=1;		/* Transmitter enable*/
	Uart0_Config.sctrl2settings.bits.bRie=1; 	/*!< Receiver Interrupt Enable for RDRF*/
	Uart0_Config.sctrl2settings.bits.bTcie=0; 	/*!< Transmission Complete Interrupt Enable for TC*/
	Uart0_Config.sctrl2settings.bits.bTie=0;		/* Transmit buffer empty interrupt enable*/
	Uart0_Config.u32SysClkHz = 24000000;   		/* Bus clock in Hz*/
	Uart0_Config.u32Baudrate = 115200;     		/* Baud rate*/
	UART_SetCallback(UART0,UART0_Isr);
	UART_Init(UART0,&Uart0_Config);
}

void UART2_Init_main(void)
{
	UART_ConfigType Uart2_Config={{0}};
	SIM_PINSEL1 &= (~SIM_PINSEL1_UART2PS_MASK); /* SIM_PINSEL1 bit 13 set 0,UART2 TX PTD7,RX PTD6 */
	Uart2_Config.sctrl1settings.bits.bM=0; 		/* 8 bit Mode */
	Uart2_Config.sctrl1settings.bits.bPe=0; 		/* Parity disable */
	Uart2_Config.bSbns=0;						/* One stop bit*/
	Uart2_Config.sctrl2settings.bits.bRe=1;		/* Receiver enable*/
	Uart2_Config.sctrl2settings.bits.bTe=1;		/* Transmitter enable*/
	Uart2_Config.sctrl2settings.bits.bRie=1; 	/*!< Receiver Interrupt Enable for RDRF*/
	Uart2_Config.sctrl2settings.bits.bTcie=0; 	/*!< Transmission Complete Interrupt Enable for TC*/
	Uart2_Config.sctrl2settings.bits.bTie=0;		/* Transmit buffer empty interrupt enable*/
	Uart2_Config.u32SysClkHz = 24000000;   		/* Bus clock in Hz*/
	Uart2_Config.u32Baudrate = 115200;     		/* Baud rate*/
	UART_SetCallback(UART2,UART2_Isr);
	UART_Init(UART2,&Uart2_Config);
	//printf("BYD_BOOTLOADER\r\n");
}

// DeInit  DeInit  DeInit  DeInit  DeInit  DeInit  DeInit
void UART0_DeInit_main(void)
{
	UART_ConfigType Uart0_Config={{0}};

	while (!(UART0->S1 & UART_S1_TC_MASK))
	{
		__NOP();
	}

	Uart0_Config.sctrl1settings.bits.bM=0; 		/* 8 bit Mode */
	Uart0_Config.sctrl1settings.bits.bPe=0; 		/* Parity disable */
	Uart0_Config.bSbns=0;						/* One stop bit*/
	Uart0_Config.sctrl2settings.bits.bRe=0;		/* Receiver enable*/
	Uart0_Config.sctrl2settings.bits.bTe=0;		/* Transmitter enable*/
	Uart0_Config.sctrl2settings.bits.bRie=0; 	/*!< Receiver Interrupt Enable for RDRF*/
	Uart0_Config.sctrl2settings.bits.bTcie=0; 	/*!< Transmission Complete Interrupt Enable for TC*/
	Uart0_Config.sctrl2settings.bits.bTie=0;		/* Transmit buffer empty interrupt enable*/
	Uart0_Config.u32SysClkHz = 24000000;   		/* Bus clock in Hz*/
	Uart0_Config.u32Baudrate = 115200;     		/* Baud rate*/
	UART_Init(UART0,&Uart0_Config);
	SIM_BASE_PTR->SCGC &= (~SIM_SCGC_UART0_MASK);
}

void UART1_LIN_DeInit_main(void)
{
	UART_ConfigType Uart1_Config={{0}};

	while (!(UART1->S1 & UART_S1_TC_MASK))
	{
		__NOP();
	}

	Uart1_Config.sctrl1settings.bits.bM=0; 		/* 8 bit Mode */
	Uart1_Config.sctrl1settings.bits.bPe=0; 		/* Parity disable */
	Uart1_Config.bSbns=0;						/* One stop bit*/
	Uart1_Config.sctrl2settings.bits.bRe=0;		/* Receiver enable*/
	Uart1_Config.sctrl2settings.bits.bTe=0;		/* Transmitter enable*/
	Uart1_Config.sctrl2settings.bits.bRie=0; 	/*!< Receiver Interrupt Enable for RDRF*/
	Uart1_Config.sctrl2settings.bits.bTcie=0; 	/*!< Transmission Complete Interrupt Enable for TC*/
	Uart1_Config.sctrl2settings.bits.bTie=0;		/* Transmit buffer empty interrupt enable*/
	Uart1_Config.u32SysClkHz = 24000000;   		/* Bus clock in Hz*/
	Uart1_Config.u32Baudrate = 19200;     		/* Baud rate*/
	UART_Init(UART1,&Uart1_Config);
	SIM_BASE_PTR->SCGC &= (~SIM_SCGC_UART1_MASK);
}

void UART2_DeInit_main(void)
{
	UART_ConfigType Uart2_Config={{0}};

	while (!(UART2->S1 & UART_S1_TC_MASK))
	{
		__NOP();
	}

	Uart2_Config.sctrl1settings.bits.bM=0; 		/* 8 bit Mode */
	Uart2_Config.sctrl1settings.bits.bPe=0; 		/* Parity disable */
	Uart2_Config.bSbns=0;						/* One stop bit*/
	Uart2_Config.sctrl2settings.bits.bRe=0;		/* Receiver enable*/
	Uart2_Config.sctrl2settings.bits.bTe=0;		/* Transmitter enable*/
	Uart2_Config.sctrl2settings.bits.bRie=0; 	/*!< Receiver Interrupt Enable for RDRF*/
	Uart2_Config.sctrl2settings.bits.bTcie=0; 	/*!< Transmission Complete Interrupt Enable for TC*/
	Uart2_Config.sctrl2settings.bits.bTie=0;		/* Transmit buffer empty interrupt enable*/
	Uart2_Config.u32SysClkHz = 24000000;   		/* Bus clock in Hz*/
	Uart2_Config.u32Baudrate = 115200;     		/* Baud rate*/
	UART_Init(UART2,&Uart2_Config);
	SIM_BASE_PTR->SCGC &= (~SIM_SCGC_UART2_MASK);
}

void FTM_Init_main(void)
{
	FTM_ConfigType FTM0_Config={0};
	FTM_ConfigType FTM1_Config={0};
	FTM_ConfigType FTM2_Config={0};
	FTM_ChParamsType FTM2CH0_Config={0};
	FTM_ChParamsType FTM2CH1_Config={0};
	FTM_ChParamsType FTM2CH5_Config={0};
	/* ================FTM0================= */
	/* if Bus clock 24000000 Hz , 24000000/128/37500 = 5 Hz = 200ms */
	FTM0_Config.modulo=48000;
	FTM0_Config.clk_source=FTM_CLOCK_SYSTEMCLOCK;
	FTM0_Config.prescaler=FTM_CLOCK_PS_DIV1;
	FTM0_Config.mode=1;
	FTM0_Config.toie=1;

	FTM_SetCallback(FTM0, &FTM0_Task);
	FTM_Init(FTM0,&FTM0_Config);

	/* ================FTM1================= */
	FTM1_Config.modulo=2400;
	FTM1_Config.clk_source=FTM_CLOCK_SYSTEMCLOCK;
	FTM1_Config.prescaler=FTM_CLOCK_PS_DIV1;
	FTM1_Config.mode=1;
	FTM1_Config.toie=1;
	FTM_SetCallback(FTM1, &FTM1_Task);
	FTM_Init(FTM1,&FTM1_Config);

	/* ================FTM2================= */
	/* if Bus clock 24000000 Hz , 24000000/128/37500 = 5 Hz = 200ms */
	FTM2_Config.modulo=FTM2_PWM_CLK;
	FTM2_Config.clk_source=FTM_CLOCK_SYSTEMCLOCK;
	FTM2_Config.prescaler=FTM_CLOCK_PS_DIV1;
	FTM2_Config.mode=1;
	FTM2_Config.toie=1;
	/* see KEA128RM-Chinese page 178 */
	SIM_PINSEL1 = (SIM_PINSEL1 & ~SIM_PINSEL1_FTM2PS0_MASK) | SIM_PINSEL1_FTM2PS0(0b01);
	SIM_PINSEL1 = (SIM_PINSEL1 & ~SIM_PINSEL1_FTM2PS1_MASK) | SIM_PINSEL1_FTM2PS1(0b01);
	SIM_PINSEL1 = (SIM_PINSEL1 & ~SIM_PINSEL1_FTM2PS5_MASK) | SIM_PINSEL1_FTM2PS5(0b0);
	FTM2CH0_Config.ctrl.bits.bMode=FTM_PWMMODE_EDGEALLIGNED;
	FTM2CH0_Config.ctrl.bits.bPWMPol=FTM_PWM_HIGHTRUEPULSE;
	FTM2CH0_Config.u16CnV=0;
	FTM2CH1_Config.ctrl.bits.bMode=FTM_PWMMODE_EDGEALLIGNED;
	FTM2CH1_Config.ctrl.bits.bPWMPol=FTM_PWM_HIGHTRUEPULSE;
	FTM2CH1_Config.u16CnV=0;
	FTM2CH5_Config.ctrl.bits.bMode=FTM_PWMMODE_EDGEALLIGNED;
	FTM2CH5_Config.ctrl.bits.bPWMPol=FTM_PWM_HIGHTRUEPULSE;
	FTM2CH5_Config.u16CnV=0;
	FTM_SetCallback(FTM2, &FTM2_Task);
	FTM_ChannelInit(FTM2,FTM_CHANNEL_CHANNEL0,FTM2CH0_Config);
	FTM_ChannelInit(FTM2,FTM_CHANNEL_CHANNEL1,FTM2CH1_Config);
	FTM_ChannelInit(FTM2,FTM_CHANNEL_CHANNEL5,FTM2CH5_Config);
	FTM_Init(FTM2,&FTM2_Config);
	FTM_SetChannelValue(FTM2, FTM_CHANNEL_CHANNEL0, (FTM2_PWM_CLK/100)*0);
	FTM_SetChannelValue(FTM2, FTM_CHANNEL_CHANNEL1, (FTM2_PWM_CLK/100)*0);
	FTM_SetChannelValue(FTM2, FTM_CHANNEL_CHANNEL5, (FTM2_PWM_CLK/100)*0);
}

void RTC_Init_main(void)
{
	RTC_ConfigType  pRTC_Config={0};

	SIM_PINSEL0 |= SIM_PINSEL_RTCPS_MASK;	/* Set RTCO to PTC5 */
	pRTC_Config.u16ModuloValue = 1;
	pRTC_Config.bClockSource   = RTC_CLKSRC_1KHZ;          /*!< clock source is 1khz */
	pRTC_Config.bClockPrescaler = RTC_CLK_PRESCALER_100;    /*!< prescaler is 100 */
	pRTC_Config.bRTCOut = RTC_OUTPUT_ENABLE;

	RTC_Init(&pRTC_Config);
}

void CRC_Init_main(void)
{
	CRC_ConfigType  sCRC_ConfigType ={0};
	CRC_ConfigType  *pCRC_Config=&sCRC_ConfigType;

	/* Initalize CRC register works under 32-bit mode */
	pCRC_Config->u32PolyData            = 0x04C11DB7;               /* Set CRC32 poly value */
	pCRC_Config->bWidth                 = CRC_WIDTH_32BIT;
	pCRC_Config->bTransposeReadType     = CRC_READ_TRANSPOSE_ALL;   /* Do not transpose in read */
	pCRC_Config->bTransposeWriteType    = CRC_WRITE_TRANSPOSE_BIT;  /* Do not transpose in write */
	pCRC_Config->bFinalXOR              = TRUE;                     /* Reverse CRC result */

	/* Begin to CRC-CCITT conversion */
	CRC_Init(pCRC_Config);                                          /* CRC module initialized as 32-bit mode */

    //u32Crc_ConverterResult = CRC_Cal32(u32SeedValue, (uint8_t *)0x00006000, 192);
    //printf("CRC32 function calculation result = 0x%x @seed = 0x%x .\n\r", (unsigned int)u32Crc_ConverterResult, (unsigned int)u32SeedValue );
	//crc_test_func();
}

void PCM_Init_main(void)
{
	/* PMC Initialization */
	PMC_ConfigType  PMC_Config={{0}};

	PMC_Config.sCtrlstatus.bits.bBandgapEn = 1;	/* Enable Bandgap buffer */
	PMC_Config.sCtrlstatus.bits.bLvdStopEn = 0;	/* Disable Low-Voltage Detect(LVD) in Stop mode */
	PMC_Config.sCtrlstatus.bits.bLvdRstEn = 0;	/* LVD events do not generate hardware resets. */
	PMC_Config.sCtrlstatus.bits.bLvdEn=1;
	PMC_Config.sCtrlstatus.bits.bLvwAck=1;
	PMC_Config.sDetectVoltSelect.bits.bLVWV=3;

	PMC_Init(PMC, &PMC_Config);					/* Initialization of PMC */
	PMC_DisableLVWInterrupt(PMC);				/* Disable hardware interrupt requests for LVWF */
}

void lvw_det(void)
{
	static uint8_t lvw_pre=0;
	uint8_t lvw_curr=0;
	PMC_ClrLVWFlag(PMC);
	lvw_curr=PMC_GetLVWFlag(PMC);
	if(lvw_curr!=0)
	{
		if(lvw_pre==0)
		{
			flash_save_pos();
		}
		printf("LVW\r\n");
	}
	lvw_pre=lvw_curr;
}

void flash_save_pos(void)
{
	uint8_t rease_flag=0;
	//FLASH_EraseSector(SET_REMEMBER_SCETOR1*512);
	if((*((uint8_t *)(SET_REMEMBER_SCETOR1*512+SAVE_BYTE_SIZE-1)))!=0xFF)
	{
		rease_flag=1;
		FLASH_EraseSector(SET_REMEMBER_SCETOR1*512);
	}
	memset(u8WriteDataBuff,0xFF,sizeof(u8WriteDataBuff));
	u8WriteDataBuff[0]=0x55;
	memcpy(&u8WriteDataBuff[2], &motor_current_position[0], sizeof(uint16_t));
	memcpy(&u8WriteDataBuff[4], &motor_current_position[1], sizeof(uint16_t));
	memcpy(&u8WriteDataBuff[6], &motor_max_position[0], sizeof(uint16_t));
	memcpy(&u8WriteDataBuff[8], &motor_max_position[1], sizeof(uint16_t));
	memcpy(&u8WriteDataBuff[10], &motor_learn_finish_flag[0], sizeof(uint8_t));
	memcpy(&u8WriteDataBuff[11], &motor_learn_finish_flag[1], sizeof(uint8_t));
	memcpy(&u8WriteDataBuff[12], &anti_clip_delta_gap1[0], sizeof(int));
	memcpy(&u8WriteDataBuff[16], &anti_clip_delta_gap1[1], sizeof(int));
	memcpy(&u8WriteDataBuff[20], &anti_clip_delta_gap2[0], sizeof(int));
	memcpy(&u8WriteDataBuff[24], &anti_clip_delta_gap2[1], sizeof(int));
	u8WriteDataBuff[SAVE_BYTE_SIZE-1]=0xAA;
	FLASH_Program(SET_REMEMBER_SCETOR1*512,u8WriteDataBuff,SAVE_BYTE_SIZE );
	while((*((uint8_t *)(SET_REMEMBER_SCETOR1*512+SAVE_BYTE_SIZE-1)))==0xFF)
	{
		__NOP();
	}
	if(rease_flag)
	{
		printf("SAVE DATA OK(erase)  M1_pos=%04X, M2_pos=%04X\r\n",
				(*((uint16_t *)(SET_REMEMBER_SCETOR1*512+2))),(*((uint16_t *)(SET_REMEMBER_SCETOR1*512+4))));
	}
	else
	{
		printf("SAVE DATA OK(no erase) M1_pos=%04X, M2_pos=%04X\r\n",
				(*((uint16_t *)(SET_REMEMBER_SCETOR1*512+2))),(*((uint16_t *)(SET_REMEMBER_SCETOR1*512+4))));
	}
}

void set_led_rgb(uint8_t r,uint8_t g,uint8_t b)
{
	led_r=r*LED_SLOW_SPEED;
	led_g=g*LED_SLOW_SPEED;
	led_b=b*LED_SLOW_SPEED;
}

void set_led_rgb_target(uint8_t r,uint8_t g,uint8_t b)
{
	led_r_target=r*LED_SLOW_SPEED;
	led_g_target=g*LED_SLOW_SPEED;
	led_b_target=b*LED_SLOW_SPEED;
}

#define KBI_USE_NUM		(5)
void KBI_Init_main(void)
{
	KBI_ConfigType  KBIConfig={{0}};
	uint8_t kbi_i=0;
	uint8_t kbi_list[KBI_USE_NUM]={2,3,17,18,19};

	/* Disable all the KBI pins */
	for (kbi_i = 0; kbi_i < KBI_MAX_PINS_PER_PORT; kbi_i++)
	{
		KBIConfig.sPin[kbi_i].bEn	 = 0;
	}

	KBIConfig.sBits.bRstKbsp   = 1;/*Writing a 1 to RSTKBSP is to clear the KBIxSP Register*/
	KBIConfig.sBits.bKbspEn   	= 1;/*The latched value in KBxSP register while interrupt flag occur to be read.*/
	KBIConfig.sBits.bMode   	= KBI_MODE_EDGE_ONLY;
	KBIConfig.sBits.bIntEn  	= 1;

	for(kbi_i=0;kbi_i<KBI_USE_NUM;kbi_i++)
	{
		/*Falling edge/low level select; KBI0_P24 channel enable(SW2 on FRDM+ board) */
		KBIConfig.sPin[kbi_list[kbi_i]].bEdge = KBI_FALLING_EDGE_LOW_LEVEL;
		KBIConfig.sPin[kbi_list[kbi_i]].bEn   = 1;
	}

	KBI_SetCallback(KBI1, &KBI1_Task);
	KBI_Init(KBI1, &KBIConfig);
}

void KBI1_Task(void)
{
	KBI_RstSP(KBI1);
	KBI_ClrFlags(KBI1);
}

void ADC_Init_main(void)
{
	ADC_ConfigType  ADC_Config={{0}};

	/* Initialization of ADC module */
	ADC_Config.u8ClockDiv = ADC_ADIV_DIVIDE_8;
	ADC_Config.u8ClockSource = CLOCK_SOURCE_BUS_CLOCK_DIVIDE_2;
	ADC_Config.u8Mode = ADC_MODE_12BIT;
	ADC_Config.sSetting.bIntEn = 1;
	ADC_Config.sSetting.bLongSampleEn = 1;
	ADC_Config.u8FiFoLevel = ADC_FIFO_LEVEL3;

	/* Disable I/O control on ADC channel 9,10,11 */
	ADC_Config.u16PinControl = ADC_BIT_CURRENT1 | ADC_BIT_CURRENT2 | ADC_BIT_BAT;

	ADC_SetCallBack(&ADC_CallBack);
	ADC_Init( ADC, &ADC_Config);
}

#define ADC_AVERAGE			(5)

void clear_adc(void)
{
	adc_current1_sum = 0;
	adc_current2_sum = 0;
	adc_bat_sum = 0;
	sum_count=0;
}

void ADC_CallBack(void)
{
	if(!ADC_IsFIFOEmptyFlag(ADC))
	{
		adc_current1_sum 	+= ADC_ReadResultReg(ADC);
		adc_current2_sum 	+= ADC_ReadResultReg(ADC);
		adc_bat_sum 		+= ADC_ReadResultReg(ADC);
		sum_count++;

		if(sum_count<ADC_AVERAGE)
		{
			ADC_SetChannel(ADC,ADC_CH_CURRENT1);
			ADC_SetChannel(ADC,ADC_CH_CURRENT2);
			ADC_SetChannel(ADC,ADC_CH_BAT);
		}
	}
}

void adc_read_function(void)
{
	if(sum_count==0)
	{
		ADC_SetChannel(ADC,ADC_CH_CURRENT1);
		ADC_SetChannel(ADC,ADC_CH_CURRENT2);
		ADC_SetChannel(ADC,ADC_CH_BAT);
	}
	if(sum_count>=ADC_AVERAGE)
	{
		adc_current1 	= adc_current1_sum	/sum_count;
		adc_current2 	= adc_current2_sum	/sum_count;
		adc_bat 		= adc_bat_sum		/sum_count;

		if(adc_current1>0x005)
		{
			//printf("adc_curr1=%04X\r\n",adc_current1);
		}
		if(adc_current2>0x005)
		{
			//printf("adc_curr2=%04X\r\n",adc_current2);
		}
#if(0)
		if(curr_not_detect_timer[DEF_MOTOR1]==0)
		{
			if(adc_current1>ADC_CURRENT_HARD_STUCK)
			{
				if(motor_output_relay_flag[DEF_MOTOR1]==DIRECTION_N)
				{
					motor_output(DEF_MOTOR1,DIRECTION_X);
					motor_hard_stuck_flag[DEF_MOTOR1][DIRECTION_N]=1;
					motor_current_position[DEF_MOTOR1] = 0x8000;
					printf("adc_curr1=%04X > 0x%04X hard stop N\r\n",adc_current1,ADC_CURRENT_HARD_STUCK);
				}
				if(motor_output_relay_flag[DEF_MOTOR1]==DIRECTION_P)
				{
					motor_output(DEF_MOTOR1,DIRECTION_X);
					motor_hard_stuck_flag[DEF_MOTOR1][DIRECTION_P]=1;
					printf("adc_curr1=%04X > 0x%04X hard stop P\r\n",adc_current1,ADC_CURRENT_HARD_STUCK);
				}
			}
		}

		if(curr_not_detect_timer[DEF_MOTOR2]==0)
		{
			if(adc_current2>ADC_CURRENT_HARD_STUCK)
			{
				if(motor_output_relay_flag[DEF_MOTOR2]==DIRECTION_N)
				{
					motor_output(DEF_MOTOR2,DIRECTION_X);
					motor_hard_stuck_flag[DEF_MOTOR2][DIRECTION_N]=1;
					motor_current_position[DEF_MOTOR2] = 0x8000;
					printf("adc_curr2=%04X > 0x%04X hard stop N\r\n",adc_current2,ADC_CURRENT_HARD_STUCK);
				}
				if(motor_output_relay_flag[DEF_MOTOR2]==DIRECTION_P)
				{
					motor_output(DEF_MOTOR2,DIRECTION_X);
					motor_hard_stuck_flag[DEF_MOTOR2][DIRECTION_P]=1;
					printf("adc_curr2=%04X > 0x%04X hard stop P\r\n",adc_current2,ADC_CURRENT_HARD_STUCK);
				}
			}
		}
#endif

		if(adc_bat!=0 && (adc_bat<ADC_BAT_8V5 || adc_bat>ADC_BAT_16V5))
		{
#if(0)
			printf("adc_bat_error_flag=1\r\n");
			printf("adc_curr1=%04X, adc_curr2=%04X, adc_bat=%04X\r\n",adc_current1,adc_current2,adc_bat);
#endif
			adc_bat_error_flag = 1;
		}
		if(adc_bat>=ADC_BAT_9V0 && adc_bat<=ADC_BAT_16V0)
		{
			adc_bat_error_flag = 0;
		}
		//printf("adc_curr1=%04X, adc_curr2=%04X, adc_bat=%04X\r\n",adc_current1,adc_current2,adc_bat);

		adc_current1_sum = 0;
		adc_current2_sum = 0;
		adc_bat_sum = 0;
		sum_count = 0;
	}
}

void Flash_Init_main(void)
{
	FLASH_Init(24000000);

	if((*((uint8_t *)(BOOT_INIT_SCETOR*512)))==BOOT_VALID_APP)
	{
		if((*((uint8_t *)(BOOT_INIT_SCETOR*512+1)))==0xF0)
		{
			uint8_t u8DataBuff[2]={BOOT_VALID_APP,0x00};
			FLASH_EraseSector(BOOT_INIT_SCETOR*0x200);
			FLASH_Program(BOOT_INIT_SCETOR*0x200,u8DataBuff,2);
			memset(uds_txdata,0xAA,sizeof(uds_txdata));
			uds_txdata[0]=(0x11+0x40);
			uds_txdata[1]=0x01;
			uds_txdata_length=2;
			uds_can_tx_flag=1;
		}
	}

	if((*((uint8_t *)(BOOT_CANID_SCETOR*512)))!=LORINSER_VERSION ||
	   (*((uint32_t *)(BOOT_CANID_SCETOR*512+4)))!=CAN_UDS_REQUEST_ID ||
	   (*((uint32_t *)(BOOT_CANID_SCETOR*512+8)))!=CAN_UDS_ACK_ID)
	{
		uint8_t u8DataBuff[1]={LORINSER_VERSION};
		uint32_t u32_req_id=CAN_UDS_REQUEST_ID;
		uint32_t u32_ack_id=CAN_UDS_ACK_ID;
		FLASH_EraseSector(BOOT_CANID_SCETOR*0x200);
		FLASH_Program(BOOT_CANID_SCETOR*0x200,u8DataBuff,1);
		FLASH_Program(BOOT_CANID_SCETOR*0x200+4,(uint8_t *)(&u32_req_id),4);
		FLASH_Program(BOOT_CANID_SCETOR*0x200+8,(uint8_t *)(&u32_ack_id),4);
		printf("\r\nSET LORINSER_VERSION=%d\r\n",LORINSER_VERSION);
	}

	uint8_t head=(*((uint8_t *)(SET_REMEMBER_SCETOR1*512+0)));
	uint8_t end=(*((uint8_t *)((SET_REMEMBER_SCETOR1*512)+SAVE_BYTE_SIZE-1)));
	if(head==0x55 && end==0xAA)
	{
		motor_current_position[0] = (*((uint16_t *)(SET_REMEMBER_SCETOR1*512+2)));
		motor_current_position[1] = (*((uint16_t *)(SET_REMEMBER_SCETOR1*512+4)));

		motor_max_position[0] = (*((uint16_t *)(SET_REMEMBER_SCETOR1*512+6)));
		motor_max_position[1] = (*((uint16_t *)(SET_REMEMBER_SCETOR1*512+8)));

		motor_learn_finish_flag[0] = (*((uint8_t *)(SET_REMEMBER_SCETOR1*512+10)));
		motor_learn_finish_flag[1] = (*((uint8_t *)(SET_REMEMBER_SCETOR1*512+11)));

		anti_clip_delta_gap1[0] = (*((int *)(SET_REMEMBER_SCETOR1*512+12)));
		anti_clip_delta_gap1[1] = (*((int *)(SET_REMEMBER_SCETOR1*512+16)));
		anti_clip_delta_gap2[0] = (*((int *)(SET_REMEMBER_SCETOR1*512+20)));
		anti_clip_delta_gap2[1] = (*((int *)(SET_REMEMBER_SCETOR1*512+24)));

		printf("Flash_Init_main SUCCESS!!  M1_pos=%04X, M2_pos=%04X\r\n",
				motor_current_position[0],motor_current_position[1]);
	}
	else
	{
		printf("Flash_Init_main ERROR!!  head=0x%02X  end=0x%02X\r\n",head,end);
	}
	FLASH_EraseSector(SET_REMEMBER_SCETOR1*512);
}

void enter_low_power_mode(void)
{
	printf("Sleep\r\n");
	set_led_rgb_target(0,0,0);
	set_led_rgb(0,0,0);
	GPIO_power_off();
	UART0_DeInit_main();
	UART1_LIN_DeInit_main();
	UART2_DeInit_main();
	while (!(FTMRE_FSTAT & FTMRE_FSTAT_CCIF_MASK))
	{
		__NOP();
	}
	MSCAN_CANRFLG |= MSCAN_CANRFLG_WUPIF_MASK;
	MSCAN_CANRIER |= MSCAN_CANRIER_WUPIE_MASK;
	MSCAN_CANCTL0 |= MSCAN_CANCTL0_WUPE_MASK;
	MSCAN_CANCTL0 |= MSCAN_CANCTL0_SLPRQ_MASK;
	while((MSCAN_CANCTL1 & MSCAN_CANCTL1_SLPAK_MASK)==0)
	{
		MSCAN_CANCTL0 |= MSCAN_CANCTL0_SLPRQ_MASK;
		MSCAN_CANTARQ |= MSCAN_CANTARQ_ABTRQ_MASK;
	}
	PMC_SetMode(PMC,PmcModeStop4);				/* Enable PMC in stop mode */
	GPIO_Init_main();
	UART0_Init_main();
	UART1_LIN_Init_main();
	UART2_Init_main();
	printf("Wakeup\r\n");
	onlylisten_timer = TIMER_SLAVE_ONLYLISTEN;
	enter_low_power_flag = POWER_STATE_ONLYLISTEN;
	lock_status=1;
	clear_adc();
}

void UART0_Isr (void)
{
	(void)UART0_S1;								/* Read UART0_S1 register*/
	UART_GetChar(UART0);
}

void UART2_Isr (void)
{
	(void)UART2_S1;								/* Read UART2_S1 register*/
	uint8_t read_char;
	read_char = UART_GetChar(UART2);
	UART_PutChar(UART2,read_char);
	if(read_char=='r')
	{
		FLASH_EraseSector(BOOT_INIT_SCETOR*0x200);
		reset_timer=10;
	}
}

void FTM0_Task(void)//1ms timer
{
	uint8_t motor=0;

	FTM_ClrOverFlowFlag(FTM0);
	led_r += (led_r_target > led_r) ? 1 : ((led_r_target < led_r) ? -1 : 0);
	led_g += (led_g_target > led_g) ? 1 : ((led_g_target < led_g) ? -1 : 0);
	led_b += (led_b_target > led_b) ? 1 : ((led_b_target < led_b) ? -1 : 0);

	if(led_r>0 || led_g>0 || led_b>0)
	{
		gpio_output_set(	MOTOR1_P_EN_PORT,	MOTOR1_P_EN_PIN,	1);
		gpio_output_set(	MOTOR1_N_EN_PORT,	MOTOR1_N_EN_PIN,	1);
	}
	else
	{
		gpio_output_set(	MOTOR1_P_EN_PORT,	MOTOR1_P_EN_PIN,	0);
		gpio_output_set(	MOTOR1_N_EN_PORT,	MOTOR1_N_EN_PIN,	0);
	}

	FTM_SetChannelValue(FTM2, FTM_CHANNEL_CHANNEL0, (FTM2_PWM_CLK/100)*(led_r/LED_SLOW_SPEED));
	FTM_SetChannelValue(FTM2, FTM_CHANNEL_CHANNEL1, (FTM2_PWM_CLK/100)*(led_g/LED_SLOW_SPEED));
	FTM_SetChannelValue(FTM2, FTM_CHANNEL_CHANNEL5, (FTM2_PWM_CLK/100)*(led_b/LED_SLOW_SPEED));

	if(io_in_process_timer>0)
	{
		io_in_process_timer--;
	}
	if(adc_process_timer>0)
	{
		adc_process_timer--;
	}
	if(printf_process_timer>0)
	{
		printf_process_timer--;
	}
	if(t61_off_timer>0)
	{
		t61_off_timer--;
		if(t61_off_timer==0 && t61_signal<=2)
		{
			//AGS OFF
			AGS_Close();
		}
	}

	if(led_blink_loop>0)
	{
		if(led_blink_timer>0)
		{
			led_blink_timer--;
		}
		else
		{
			led_blink_timer=500;
			led_blink_loop--;
			if(led_blink_loop%2==1)
			{
				set_led_rgb_target(30,0,30);
				set_led_rgb(30,0,30);
			}
			if(led_blink_loop%2==0)
			{
				set_led_rgb_target(0,0,0);
				set_led_rgb(0,0,0);
			}
		}
	}
	else
	{
		led_blink_timer=0;
	}

	if(lin_process_timer>0)
	{
		lin_process_timer--;
	}
	if(reset_timer>0)
	{
		reset_timer--;
		if(reset_timer==0)
		{
			NVIC_SystemReset();
		}
	}
	if(SecurityUnlocked_timer>0)
	{
		SecurityUnlocked_timer--;
		if(SecurityUnlocked_timer==0)
		{
			current_SecurityUnlocked=0;
		}
	}
	if(onlylisten_timer>0)
	{
		onlylisten_timer--;
		if(onlylisten_timer==0)
		{
			if(enter_low_power_flag == POWER_STATE_ONLYLISTEN)
			{
				enter_low_power_flag = POWER_STATE_LOWPOWER;
			}
		}
	}
	if(motor_start_delay_timer>0)
	{
		motor_start_delay_timer--;
		if(motor_start_delay_timer==0)
		{
			if(auto_next_way[DEF_MOTOR1]==DIRECTION_N)
			{
				auto_step[DEF_MOTOR2]=AUTO_OPEN_START;
			}
			if(auto_next_way[DEF_MOTOR1]==DIRECTION_P)
			{
				auto_step[DEF_MOTOR2]=AUTO_CLOSE_START;
			}
		}
	}

	for(motor=0;motor<TOTAL_MOTOR_NUM;motor++)
	{
		uint8_t way=motor_output_relay_flag[motor];
		if(way!=DIRECTION_X)
		{
			if(hall_not_detect_timer[motor][way]>0)
			{
				hall_not_detect_timer[motor][way]--;
			}
			else
			{
				hall_not_detect_timer[motor][way]=HARD_STUCK_TIME;
				motor_output_relay_flag[motor]=DIRECTION_X;
				motor_hard_stuck_flag[motor][way]=1;
				if(way==DIRECTION_N)
				{
					printf("hard stop set pos=0x8000\r\n");
					motor_current_position[motor] = 0x8000;
				}
				/*
				if(way==DIRECTION_P && auto_step[motor]==AUTO_OPEN_WAIT_END)
				{
					printf("hard stop set pos=0x%04X\r\n",motor_max_position[motor]);
					motor_current_position[motor] = motor_max_position[motor];
				}
				*/
			}
		}
		if(curr_not_detect_timer[motor]>0)
		{
			curr_not_detect_timer[motor]--;
		}
		if(motor_slide_timer[motor]>0)
		{
			motor_slide_timer[motor]--;
			if(motor_slide_timer[motor]==0)
			{
				motor_output_relay_flag[motor]=DIRECTION_X;
			}
		}
		if(anti_clip_reverse_timer[motor]>0)
		{
			anti_clip_reverse_timer[motor]--;
		}
	}
}

void FTM1_Task(void)//0.1ms timer
{
	hall_detect();
	if(cantx_timeout_timer>0)
	{
		cantx_timeout_timer--;
	}
	FTM_ClrOverFlowFlag(FTM1);
}

void FTM2_Task(void)//1ms timer
{
	FTM_ClrOverFlowFlag(FTM2);
}

void NMI_Handler(void)
{
	SIM_SOPT0&= ~SIM_SOPT0_NMIE_MASK;
}

void JumpToUserApplication( unsigned int userSP,  unsigned int userStartup)
{
	/* Check if Entry address is erased and return if erased */
	if(userSP == 0xFFFFFFFF){
		return;
	}

	/* Set up stack pointer */
	__asm("msr msp, r0");
	__asm("msr psp, r0");

	/* Relocate vector table */
	SCB->VTOR = (uint32_t)APP_START_ADDRESS;

	/* Jump to application PC (r1) */
	__asm("mov pc, r1");
}

void gpio_output_set(GPIO_Type * port,uint8_t pin,uint8_t set)
{
	if(set)
	{
		OUTPUT_CLEAR(port,pin);
	}
	else
	{
		OUTPUT_SET(port,pin);
	}
}