#include "include.h"

u8 CAN_DATA[8];
u8 CAN_DATA1[8];
u8 CAN_DATA2[8];
u8 CAN_DATA3[8];
u8 CAN_DATA4[8];
u8 CAN_DATA5[8];
u8 CAN_DATA6[8];

u8 CAN_DATA_CNT;

u32 Init_Flash_Data = 0x12345678;

u8 CAN_RecData[CAN_RecDataMax];
u8 CAN_RecData_i;
u8 CAN_RecData_Toggle;
u8 CAN_RecData_TID;

void CAN_GPIO_Config(u32 RCC_APBnPeriphn, GPIO_TypeDef* GPIOx, u16 GPIO_Pin, u16 GPIO_PinSource, u8 GPIO_AF)
{
	RCC_AHB1PeriphClockCmd(RCC_APBnPeriphn, ENABLE);
	//初始化GPIO
	//引脚复用映射配置
	GPIO_PinAFConfig(GPIOx,GPIO_PinSource,GPIO_AF);//PA11复用为CAN1
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Pin   = GPIO_Pin;
	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;			//复用功能
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;		//推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;			//上拉
	GPIO_Init(GPIOx,&GPIO_InitStructure);			//初始化PD0,PD1
}

void CAN_NVIC_Config(u8 NVIC_IRQChannel, u8 pre, u8 sub)
{
	NVIC_InitTypeDef  NVIC_InitStructure;
  NVIC_InitStructure.NVIC_IRQChannel                   = NVIC_IRQChannel;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = pre;     // 主优先级为1
  NVIC_InitStructure.NVIC_IRQChannelSubPriority        = sub;            // 次优先级为0
  NVIC_InitStructure.NVIC_IRQChannelCmd                = ENABLE;
  NVIC_Init(&NVIC_InitStructure);
}

void CAN_Mod_Config(u32 RCC_APB1Periph, CAN_TypeDef* CANx, u32 EXTID, u32 MaskID)
{
	//u32 EXTID1    = 0X014E550A;
	//u32 MaskId_ID = 0XFFFF0000;
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph, ENABLE);
	
	CAN_InitTypeDef CAN_InitStructure;
	CAN_InitStructure.CAN_TTCM = DISABLE;     		//非时间触发通信模式
	CAN_InitStructure.CAN_ABOM = DISABLE;     		//软件自动离线管理  
	CAN_InitStructure.CAN_AWUM = DISABLE;   			//睡眠模式通过软件唤醒
	CAN_InitStructure.CAN_NART = ENABLE;      		//禁止报文自动传送
	CAN_InitStructure.CAN_RFLM = DISABLE;     		//报文不锁定,新的覆盖旧的
	CAN_InitStructure.CAN_TXFP = DISABLE;     		//优先级由报文标识符决定
	CAN_InitStructure.CAN_Mode = CAN_Mode_Normal; //模式设置 CAN_Mode_Normal
	CAN_InitStructure.CAN_SJW  = CAN_SJW_1tq;			//重新同步跳跃宽度为个时间单位
	CAN_InitStructure.CAN_BS1  = CAN_BS1_8tq;			//时间段1占用8个时间单位
	CAN_InitStructure.CAN_BS2  = CAN_BS2_5tq;			//时间段2占用5个时间单位
	CAN_InitStructure.CAN_Prescaler = 6;					//分频系数(Fdiv)6  500K
	CAN_Init(CANx, &CAN_InitStructure);     		  //初始化CAN1
	/*
	CAN_FilterInitTypeDef CAN_FilterInitStructure;
	CAN_FilterInitStructure.CAN_FilterNumber         = 0;//过滤器0
	CAN_FilterInitStructure.CAN_FilterMode           = CAN_FilterMode_IdList;// CAN_FilterMode_IdMask
	CAN_FilterInitStructure.CAN_FilterScale          = CAN_FilterScale_32bit;//32位
	CAN_FilterInitStructure.CAN_FilterIdHigh         = ((EXTID<<3)>>16) & 0xffff;//32位ID
	CAN_FilterInitStructure.CAN_FilterIdLow          = ((EXTID<<3)& 0xffff) | CAN_ID_EXT;
	CAN_FilterInitStructure.CAN_FilterMaskIdHigh 		 = ((MaskID<<3)>>16) & 0xffff;//32位ID
	CAN_FilterInitStructure.CAN_FilterMaskIdLow      = ((MaskID<<3)& 0xffff) | CAN_ID_EXT;
	CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_Filter_FIFO0;//FIFO0
	CAN_FilterInitStructure.CAN_FilterActivation     = ENABLE;//激活过滤器0
	CAN_FilterInit(&CAN_FilterInitStructure);//滤波器初始化
	*/
	CAN_FilterInitTypeDef CAN_FilterInitStructure;
	CAN_FilterInitStructure.CAN_FilterNumber         = 0;//过滤器0
	CAN_FilterInitStructure.CAN_FilterMode           = CAN_FilterMode_IdList;// CAN_FilterMode_IdMask
	CAN_FilterInitStructure.CAN_FilterScale          = CAN_FilterScale_32bit;//32位
	CAN_FilterInitStructure.CAN_FilterIdHigh         = ((EXTID<<3)>>16) & 0xffff;//32位ID
	CAN_FilterInitStructure.CAN_FilterIdLow          = ((EXTID<<3)& 0xffff) | CAN_ID_EXT;
	CAN_FilterInitStructure.CAN_FilterMaskIdHigh 		 = ((MaskID<<3)>>16) & 0xffff;//32位ID
	CAN_FilterInitStructure.CAN_FilterMaskIdLow      = ((MaskID<<3)& 0xffff) | CAN_ID_EXT;
	CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_Filter_FIFO0;//FIFO0
	CAN_FilterInitStructure.CAN_FilterActivation     = ENABLE;//激活过滤器0
	CAN_FilterInit(&CAN_FilterInitStructure);//滤波器初始化
	
	CAN_ITConfig(CANx,CAN_IT_FMP0,ENABLE);//FIFO0 允许 消息挂号中断.  
}

u8 CAN1_Send_Msg(u8* msg,u32 Can_ID,u8 len)
{
  u8 mbox;
  u16 i = 0;
  CanTxMsg TxMessage;
	
  TxMessage.StdId = 0;      				// 标准标识符为0
	TxMessage.ExtId = Can_ID;//0x1f4e5301  0x1f4e5701 // 设置扩展标示符（29位）
	
  TxMessage.IDE = CAN_ID_EXT;       // 使用扩展标识符
  TxMessage.RTR = CAN_RTR_DATA;     // 消息类型为数据帧，一帧8位
  TxMessage.DLC = len;         			// 发送两帧信息
  
	for(i=0;i<len;i++)
	{
		TxMessage.Data[i] = msg[i];  			// 第一帧信息      
	}
  mbox = CAN_Transmit(CAN1,&TxMessage); 
  
	i=0;
  while((CAN_TransmitStatus(CAN1,mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))
	{
		i++;//等待发送结束
	}
	
  if(i>=0XFFF)return 1;
	return 0;
}

u8 CAN1_Receive_Msg(u8 *buf, u32 *id)
{
  u32 i;
  CanRxMsg RxMessage;
  
	if(CAN_MessagePending(CAN1,CAN_FIFO0)==0)return 0; //没有接收到数据,直接退出 
  
	CAN_Receive(CAN1, CAN_FIFO0,&RxMessage);					//读取数据   
	
	*id = RxMessage.ExtId;
	
  for(i=0;i<RxMessage.DLC;i++)
  {
		buf[i]=RxMessage.Data[i];
  }
  return RxMessage.DLC;
}

//Flash参数发送
void Send_Flash_Set(u8 num, u8 flag, float value)
{
	int temp;
	CAN_DATA[0] = num; //参数标号
	CAN_DATA[1] = flag;//命令编号
	
	temp = (int)value;//00000123.00 
	
	/*
	CAN_DATA[2] = (temp/10000000)<<4 | (temp%10000000)/1000000;
	
	temp = temp%1000000;//000123.00 
	CAN_DATA[3] = (temp/100000)<<4   | (temp%100000)  /10000;
	
	temp = temp%10000;//0123.00
	CAN_DATA[4] = (temp/1000)<<4     | (temp%1000)    /100;
	
	temp = temp%100;//23.00
	CAN_DATA[5] = (temp/10)<<4       | (temp%10);
	
	temp        = (value-(int)value)*100;
	CAN_DATA[6] = ((int)temp/10)<<4        | ((int)temp%10);
	*/
	//12345678
	CAN_DATA[2] = (temp/1000000);
	temp = temp%1000000;
	
	CAN_DATA[3] = (temp/10000);
	temp = temp%10000;
	
	CAN_DATA[4] = (temp/100);
	temp = temp%100;
	
	CAN_DATA[5] = temp;
	
	temp = (value-(int)value)*100;
	
	CAN_DATA[6] = temp;
	
	CAN_DATA[7] = 0xC0 + CAN_DATA_CNT++;
	if(CAN_DATA_CNT>=32) CAN_DATA_CNT = 0;
	
	CAN1_Send_Msg(CAN_DATA, CAN_Send_Flash_ID, 8);
}

float Rec_Flash_Set(u8 data[])
{
	float value = 0;
	
	value = (data[2]/16)*10000000 +
					(data[2]%16)*1000000  +
					(data[3]/16)*100000   +
					(data[3]%16)*10000    +
					(data[4]/16)*1000     +
					(data[4]%16)*100      +
					(data[5]/16)*10       +
					(data[5]%16)          +
					(data[6]/16)*0.1      +
					(data[6]%16)*0.01     ;
	
	return value;
}

void Send_CAN_DATA1(void)
{
	CAN_DATA1[0] = (u8)right_thruster.usart_data.rpm;
	CAN_DATA1[1] = (u8)((u16)(right_thruster.usart_data.rpm)>>8)|((u8)(10-right_thruster.twin_data.speed_lim))<<4;
	CAN_DATA1[2] = (u8)left_thruster.usart_data.rpm;
	CAN_DATA1[3] = (u8)((u16)(left_thruster.usart_data.rpm)>>8)|((u8)(10-left_thruster.twin_data.speed_lim))<<4;
//	CAN_DATA1[0] = 100;
//	CAN_DATA1[1] = 0;
//	CAN_DATA1[2] = 100;
//	CAN_DATA1[3] = 0;
	
	CAN_DATA1[4] = 0;
	CAN_DATA1[5] = 0;
	
	CAN_DATA1[6] = 0;
	CAN_DATA1[7] = 0xC0 + CAN_DATA_CNT++;
	if(CAN_DATA_CNT>=32) CAN_DATA_CNT = 0;
	
	CAN1_Send_Msg(CAN_DATA1, CAN_Send_Speed_ID, 8);
}

void Send_CAN_DATA2(void)
{
	CAN_DATA2[0] = (u8)left_thruster.pid_data.aim_speed;
	CAN_DATA2[1] = (u8)(left_thruster.pid_data.aim_speed>>8);
	CAN_DATA2[2] = (u8)right_thruster.pid_data.aim_speed;
	CAN_DATA2[3] = (u8)(right_thruster.pid_data.aim_speed>>8);
	CAN_DATA2[4] = (u8)left_thruster.pid_data.dshot_pwm;
	CAN_DATA2[5] = (u8)(left_thruster.pid_data.dshot_pwm>>8);
	
	CAN_DATA2[6] = 3;
	CAN_DATA2[7] = 0xC0 + CAN_DATA_CNT++;
	if(CAN_DATA_CNT>=32) CAN_DATA_CNT = 0;
	
	CAN1_Send_Msg(CAN_DATA2, CAN_Send_Speed_ID, 8);
}

void Send_CAN_DATA3(void)
{
	CAN_DATA3[0] = (u8)right_thruster.pid_data.dshot_pwm;
	CAN_DATA3[1] = (u8)(right_thruster.pid_data.dshot_pwm>>8);
	CAN_DATA3[2] = 0;
	CAN_DATA3[3] = 0;
	CAN_DATA3[4] = (u8)(left_track_motor.usart_data.rpm);
	CAN_DATA3[5] = (u8)((u16)(left_track_motor.usart_data.rpm)>>8) | ((u8)(10-left_track_motor.twin_data.speed_lim))<<4;
	
	CAN_DATA3[6] = 6;
	CAN_DATA3[7] = 0xC0 + CAN_DATA_CNT++;
	if(CAN_DATA_CNT>=32) CAN_DATA_CNT = 0;
	
	CAN1_Send_Msg(CAN_DATA3, CAN_Send_Speed_ID, 8);
}

void Send_CAN_DATA4(void)
{
	CAN_DATA4[0] = (u8)(right_track_motor.usart_data.rpm);
	CAN_DATA4[1] = (u8)((u16)(right_track_motor.usart_data.rpm)>>8) | ((u8)(10-right_track_motor.twin_data.speed_lim))<<4;
	CAN_DATA4[2] = 0;
	CAN_DATA4[3] = 0;
	CAN_DATA4[4] = 0;
	CAN_DATA4[5] = 0;
	
	CAN_DATA4[6] = 9;
	CAN_DATA4[7] = 0xC0 + CAN_DATA_CNT++; 
	if(CAN_DATA_CNT>=32) CAN_DATA_CNT = 0;
	
	CAN1_Send_Msg(CAN_DATA4, CAN_Send_Speed_ID, 8);
}

void Send_CAN_DATA5(void)
{	
	CAN_DATA5[0] = (u8)(Voltage*10);
	CAN_DATA5[1] = (u8)((u16)(Voltage*10)>>8);
	CAN_DATA5[2] = (u8)(temperature*10);
	CAN_DATA5[3] = (u8)((u16)(temperature*10)>>8);
	
	CAN_DATA5[4] = Motor_err_flag;
	CAN_DATA5[5] = Motor_err_flag>>8;
	
	CAN_DATA5[6] = 12;
	CAN_DATA5[7] = 0xC0 + CAN_DATA_CNT++;
	if(CAN_DATA_CNT>=32) CAN_DATA_CNT = 0;
	
	CAN1_Send_Msg(CAN_DATA5, CAN_Send_Speed_ID, 8);
}

void Send_CAN_DATA6(void)
{
	CAN_DATA6[0] = (u8)light_pwm;
	CAN_DATA6[1] = (u8)(light_pwm>>8);
	CAN_DATA6[2] = usbl_opendown;
	CAN_DATA6[3] = 0;
	
	if(light_err>0) CAN_DATA6[4] = 1;
	else            CAN_DATA6[4] = 0;
	
	CAN_DATA6[5] = humidity_err;
	CAN_DATA6[6] = 15;
	CAN_DATA6[7] = 0xC0 + CAN_DATA_CNT++;
	if(CAN_DATA_CNT>=32) CAN_DATA_CNT = 0;
	
	CAN1_Send_Msg(CAN_DATA6, CAN_Send_Speed_ID, 8);
}

void Send_CAN_MOTOR1DATA(void)
{
	static u8 tail1 = 0;
	
	hv_motor1_data[7] = tail1 + 0xC0;
	
	tail1++;
	if(tail1>31) tail1 = 0;
	
	CAN1_Send_Msg(hv_motor1_data, 0x014E650B, 8);
}

void Send_CAN_MOTOR2DATA(void)
{
	static u8 tail2 = 0;
	
	hv_motor2_data[7] = tail2 + 0xC0;
	
	tail2++;
	if(tail2>31) tail2 = 0;
	
	CAN1_Send_Msg(hv_motor2_data, 0x014E660C, 8);
}

void Send_CAN_MOTOR3DATA(void)
{
	static u8 tail3 = 0;
	
	hv_motor3_data[7] = tail3 + 0xC0;
	
	tail3++;
	if(tail3>31) tail3 = 0;
	
	CAN1_Send_Msg(hv_motor3_data, 0x014E670D, 8);
}