/**
  ****************************(C) COPYRIGHT ZhouCc****************************
  * @file       djiMotor.c/h
  * @brief      大疆电机接收发送程序
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023.12.11			ZhouCc					完成
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT ZhouCc****************************
  */

#include "djiMotor.h"
#include "can_it.h"
#include "can_init.h"
#if FREERTOS_DJI == 1
#include "TopDefine.h"
#include<cmsis_os2.h>
#include "FreeRTOS.h"

#include "odrive_can.h"


static CAN_TxHeaderTypeDef vesc_tx_message;
static uint8_t  vesc_send_data[4];

#endif

//motor data read
#define get_motor_measure(ptr, data)                    								\
    {                                                                 	\
        (ptr)->last_ecd = (ptr)->ecd;                              			\
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6]; 																	\
				(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt);																	\
				(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt);																\
				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508;														\
    }//ptr指向电机测量数据结构体的指针,data包含电机测量数据的数组.
/*(ptr)->real_angle = (ptr)->real_angle % 360;	*/
#define get_motor_offset(ptr, data)														\
		{																																		\
				(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
				(ptr)->offset_ecd = (ptr)->ecd;																	\
		}
#define get_6020_motor_measure(ptr, data)                     								\
    {                                                                 	\
        (ptr)->last_ecd = (ptr)->ecd;                              			\
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6]; 																	\
				(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt);																	\
				(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt);																\
				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020;														\
    }                       
#define get_2006_motor_measure(ptr, data)                     								\
    {                                                                 	\
        (ptr)->last_ecd = (ptr)->ecd;                              			\
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6]; 																	\
				(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt);																	\
				(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt);																\
				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006;														\
    }     
		//		解析出初始编码器值			
// #define get_5065motor_measure(ptr, data)                                    \
// 		{                                                                   \
//         (ptr)->last_ecd = (ptr)->ecd;                                   \
//         (ptr)->ecd = (float)((data)[0] << 4 | (data)[1] <<3 |(data)[2] < 2 |(data)[3]);            \
//         (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);;      \
//     }
/*(ptr)->real_angle = (ptr)->real_angle % 360;	*/
	
static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
  const uint8_t *raw)
{
  if (feedback == NULL || raw == NULL) return;												 
  union 
  {
  int x;
  uint8_t data[4];
  }speed;

  speed.data[0]= raw[3];
  speed.data[1]= raw[2];
  speed.data[2]= raw[1];
  speed.data[3]= raw[0];
  feedback->rotor_speed = speed.x;
  union
  {
  int16_t y;
  uint8_t dat[2];
  }current;
  current.dat[0]= raw[5];
  current.dat[1]= raw[4];
  feedback->torque_current =(fp32)current.y/10;
}


#if DEBUG == 1
motor_measure_t motor_chassis[5];
CAN_MotorFeedback_t motor_6384;

#else
static motor_measure_t motor_chassis[5];
#endif
static CAN_TxHeaderTypeDef  tx_meessage_1ff;
static uint8_t              can_send_data_1ff[8];
static CAN_TxHeaderTypeDef  tx_meessage_2ff;
static uint8_t              can_send_data_2ff[8];
static CAN_TxHeaderTypeDef  tx_message_200;
static uint8_t              can_send_data_200[8];

CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8];


//小米电机
CAN_RxHeaderTypeDef rxMsg;//发送接收结构体
CAN_TxHeaderTypeDef txMsg;//发送配置结构体

uint32_t Motor_Can_ID;    //接收数据电机ID
uint8_t byte[4];          //转换临时数据
uint32_t send_mail_box = {0};//NONE
    
#define can_txd() HAL_CAN_AddTxMessage(&hcan1, &txMsg, tx_data, &send_mail_box)//CAN发送宏定义

MI_Motor mi_motor[4];//预先定义四个小米电机

/**
  * @brief          浮点数转4字节函数
  * @param[in]      f:浮点数
  * @retval         4字节数组
  * @description  : IEEE 754 协议
  */
static uint8_t* Float_to_Byte(float f)
{
	unsigned long longdata = 0;
	longdata = *(unsigned long*)&f;       
	byte[0] = (longdata & 0xFF000000) >> 24;
	byte[1] = (longdata & 0x00FF0000) >> 16;
	byte[2] = (longdata & 0x0000FF00) >> 8;
	byte[3] = (longdata & 0x000000FF);
	return byte;
}

/**
  * @brief          小米电机回文16位数据转浮点
  * @param[in]      x:16位回文
  * @param[in]      x_min:对应参数下限
  * @param[in]      x_max:对应参数上限
  * @param[in]      bits:参数位数
  * @retval         返回浮点值
  */
static float uint16_to_float(uint16_t x,float x_min,float x_max,int bits)
{
    uint32_t span = (1 << bits) - 1;
    float offset = x_max - x_min;
    return offset * x / span + x_min;
}

/**
  * @brief          小米电机发送浮点转16位数据
  * @param[in]      x:浮点
  * @param[in]      x_min:对应参数下限
  * @param[in]      x_max:对应参数上限
  * @param[in]      bits:参数位数
  * @retval         返回浮点值
  */
static int float_to_uint(float x, float x_min, float x_max, int bits)
{
  float span = x_max - x_min;
  float offset = x_min;
  if(x > x_max) x=x_max;
  else if(x < x_min) x= x_min;
  return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}

/**
  * @brief          写入电机参数
  * @param[in]      Motor:对应控制电机结构体
  * @param[in]      Index:写入参数对应地址
  * @param[in]      Value:写入参数值
  * @param[in]      Value_type:写入参数数据类型
  * @retval         none
  */
static void Set_Motor_Parameter(MI_Motor *Motor,uint16_t Index,float Value,char Value_type){
	uint8_t tx_data[8];
	txMsg.ExtId = Communication_Type_SetSingleParameter<<24|Master_CAN_ID<<8|Motor->CAN_ID;
	tx_data[0]=Index;
	tx_data[1]=Index>>8;
	tx_data[2]=0x00;
	tx_data[3]=0x00;
	if(Value_type == 'f'){
		Float_to_Byte(Value);
		tx_data[4]=byte[3];
		tx_data[5]=byte[2];
		tx_data[6]=byte[1];
		tx_data[7]=byte[0];		
	}
	else if(Value_type == 's'){
		tx_data[4]=(uint8_t)Value;
		tx_data[5]=0x00;
		tx_data[6]=0x00;
		tx_data[7]=0x00;				
	}
	can_txd();	
}

/**
  * @brief          提取电机回复帧扩展ID中的电机CANID
  * @param[in]      CAN_ID_Frame:电机回复帧中的扩展CANID   
  * @retval         电机CANID
  */
static uint32_t Get_Motor_ID(uint32_t CAN_ID_Frame)
{
	return (CAN_ID_Frame&0xFFFF)>>8;
}

/**
  * @brief          电机回复帧数据处理函数
  * @param[in]      Motor:对应控制电机结构体   
  * @param[in]      DataFrame:数据帧
  * @param[in]      IDFrame:扩展ID帧
  * @retval         None
  */
static void Motor_Data_Handler(MI_Motor *Motor,uint8_t DataFrame[8],uint32_t IDFrame)
{	
		Motor->Angle=uint16_to_float(DataFrame[0]<<8|DataFrame[1],MIN_P,MAX_P,16);
		Motor->Speed=uint16_to_float(DataFrame[2]<<8|DataFrame[3],V_MIN,V_MAX,16);			
		Motor->Torque=uint16_to_float(DataFrame[4]<<8|DataFrame[5],T_MIN,T_MAX,16);				
		Motor->Temp=(DataFrame[6]<<8|DataFrame[7])*Temp_Gain;
		Motor->error_code=(IDFrame&0x1F0000)>>16;	
}

/**
  * @brief          小米电机ID检查
  * @param[in]      id:  控制电机CAN_ID【出厂默认0x7F】
  * @retval         none
  */
void chack_cybergear(uint8_t ID)
{
    uint8_t tx_data[8] = {0};
    txMsg.ExtId = Communication_Type_GetID<<24|Master_CAN_ID<<8|ID;
    can_txd();
}

/**
  * @brief          使能小米电机
  * @param[in]      Motor:对应控制电机结构体   
  * @retval         none
  */
void start_cybergear(MI_Motor *Motor)
{
    uint8_t tx_data[8] = {0}; 
    txMsg.ExtId = Communication_Type_MotorEnable<<24|Master_CAN_ID<<8|Motor->CAN_ID;
    can_txd();
}

/**
  * @brief          停止电机
  * @param[in]      Motor:对应控制电机结构体   
  * @param[in]      clear_error:清除错误位(0 不清除 1清除)
  * @retval         None
  */
void stop_cybergear(MI_Motor *Motor,uint8_t clear_error)
{
	uint8_t tx_data[8]={0};
	tx_data[0]=clear_error;//清除错误位设置
	txMsg.ExtId = Communication_Type_MotorStop<<24|Master_CAN_ID<<8|Motor->CAN_ID;
    can_txd();
}

/**
  * @brief          设置电机模式(必须停止时调整!)
  * @param[in]      Motor:  电机结构体
  * @param[in]      Mode:   电机工作模式(1.运动模式Motion_mode 2. 位置模式Position_mode 3. 速度模式Speed_mode 4. 电流模式Current_mode)
  * @retval         none
  */
void set_mode_cybergear(MI_Motor *Motor,uint8_t Mode)
{	
	Set_Motor_Parameter(Motor,Run_mode,Mode,'s');
}

/**
  * @brief          电流控制模式下设置电流
  * @param[in]      Motor:  电机结构体
  * @param[in]      Current:电流设置
  * @retval         none
  */
void set_current_cybergear(MI_Motor *Motor,float Current)
{
	Set_Motor_Parameter(Motor,Iq_Ref,Current,'f');
}

/**
  * @brief          设置电机零点
  * @param[in]      Motor:  电机结构体
  * @retval         none
  */
void set_zeropos_cybergear(MI_Motor *Motor)
{
	uint8_t tx_data[8]={0};
	tx_data[0] = 1;
	txMsg.ExtId = Communication_Type_SetPosZero<<24|Master_CAN_ID<<8|Motor->CAN_ID;
	can_txd();		
}

/**
  * @brief          设置电机CANID
  * @param[in]      Motor:  电机结构体
  * @param[in]      Motor:  设置新ID
  * @retval         none
  */
void set_CANID_cybergear(MI_Motor *Motor,uint8_t CAN_ID)
{
	uint8_t tx_data[8]={0};
	txMsg.ExtId = Communication_Type_CanID<<24|CAN_ID<<16|Master_CAN_ID<<8|Motor->CAN_ID;
    Motor->CAN_ID = CAN_ID;//将新的ID导入电机结构体
    can_txd();	
}
/**
  * @brief          小米电机初始化
  * @param[in]      Motor:  电机结构体
  * @param[in]      Can_Id: 小米电机ID(默认0x7F)
  * @param[in]      Motor_Num: 电机编号
  * @param[in]      mode: 电机工作模式(0.运动模式Motion_mode 1. 位置模式Position_mode 2. 速度模式Speed_mode 3. 电流模式Current_mode)
  * @retval         none
  */
void init_cybergear(MI_Motor *Motor,uint8_t Can_Id, uint8_t mode)
{
    txMsg.StdId = 0;            //配置CAN发送:标准帧清零 
    txMsg.ExtId = 0;            //配置CAN发送:扩展帧清零     
    txMsg.IDE = CAN_ID_EXT;     //配置CAN发送:扩展帧
    txMsg.RTR = CAN_RTR_DATA;   //配置CAN发送:数据帧
    txMsg.DLC = 0x08;           //配置CAN发送:数据长度
    
	Motor->CAN_ID=Can_Id;       //ID设置 
	set_mode_cybergear(Motor,mode);//设置电机模式
	start_cybergear(Motor);        //使能电机
}

/**
  * @brief          小米运控模式指令
  * @param[in]      Motor:  目标电机结构体
  * @param[in]      torque: 力矩设置[-12,12] N*M
  * @param[in]      MechPosition: 位置设置[-12.5,12.5] rad
  * @param[in]      speed: 速度设置[-30,30] rpm
  * @param[in]      kp: 比例参数设置
  * @param[in]      kd: 微分参数设置
  * @retval         none
  */
void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd)
{   
    uint8_t tx_data[8];//发送数据初始化
    //装填发送数据
    tx_data[0]=float_to_uint(MechPosition,P_MIN,P_MAX,16)>>8;  
    tx_data[1]=float_to_uint(MechPosition,P_MIN,P_MAX,16);  
    tx_data[2]=float_to_uint(speed,V_MIN,V_MAX,16)>>8;  
    tx_data[3]=float_to_uint(speed,V_MIN,V_MAX,16);  
    tx_data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16)>>8;  
    tx_data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16);  
    tx_data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16)>>8;  
    tx_data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16); 
    
    txMsg.ExtId = Communication_Type_MotionControl<<24|float_to_uint(torque,T_MIN,T_MAX,16)<<8|Motor->CAN_ID;//装填扩展帧数据
    can_txd();
}


/**
 * @brief           数据处理函数
 * @param[in]       none
 * @retval          none
*/
void djiMotorEncode()
{
  if(dji_rx_header.IDE == CAN_ID_STD)
  {
    switch (dji_rx_header.StdId)
    {
    case 0x201:
    case 0x202:
    case 0x203:
    case 0x204:
    case 0x205:
    case 0x206:
    case 0x207:
    {
        static uint8_t i = 0;
        //get motor id
        i = dji_rx_header.StdId - 0x201;
        if(motor_chassis[i].msg_cnt<=50)
        {
            motor_chassis[i].msg_cnt++;
            get_motor_offset(&motor_chassis[i], dji_rx_data);
        }else{
          get_motor_measure(&motor_chassis[i], dji_rx_data);
        }
        break;
    }
  
            
    default:
    {
        break;
    }
    }
  }
if(dji_rx_header.IDE == CAN_ID_EXT)
{
  Motor_Can_ID=Get_Motor_ID(dji_rx_header.ExtId);//首先获取回传电机ID信息  
	
    switch(Motor_Can_ID)                  
    {
       
        case 0x01:  
            if(dji_rx_header.ExtId>>24 != 0)               //检查是否为广播模式
                Motor_Data_Handler(&mi_motor[0],dji_rx_data,dji_rx_header.ExtId);
            else 
                mi_motor[0].MCU_ID = dji_rx_data[0];
            break;           
        default:
            break;		
        }		
}




}

void vescMotorEncode()
{
  switch (dji_rx_header.ExtId) {
		
    case CAN_VESC5065_M1_MSG1:
      // 存储消息到对应的电机结构体中
      CAN_VescMotor_Decode_1(&motor_6384, dji_rx_data);
      break;
      
    default:
      break;
  }

}

#if FREERTOS_DJI == 0
/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
    
    djiMotorEncode();
    
}
#else
static osEventFlagsId_t eventReceive;//事件标志
/**
  * @brief          自定义大疆电机回调函数
  * @param[in]      none
  * @retval         none
  */
void Dji_Motor_CB()
{
  HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
  
  osEventFlagsSet(eventReceive, EVENT_CAN);
}

/**
 * @brief     大疆电机初始化
 * @param     none
 * @retval    none
*/
void djiInit(void)
{
  BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
                          Dji_Motor_CB);
  can_filter_init();
}

/**
 * @brief 等待新数据
*/
uint32_t waitNewDji() 
{
  return osEventFlagsWait(
				eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
}
#endif

/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      motor1: (0x205) 电机控制电流
  * @param[in]      motor2: (0x206) 电机控制电流
  * @param[in]      motor3: (0x207) 电机控制电流
  * @param[in]      motor4: (0x208) 电机控制电流
  * @retval         none
  */
extern void CAN_cmd_1FF(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_meessage_1ff.StdId = 0x1FF;
    tx_meessage_1ff.IDE = CAN_ID_STD;
    tx_meessage_1ff.RTR = CAN_RTR_DATA;
    tx_meessage_1ff.DLC = 0x08;
    can_send_data_1ff[0] = (motor1 >> 8);
    can_send_data_1ff[1] = motor1;
    can_send_data_1ff[2] = (motor2 >> 8);
    can_send_data_1ff[3] = motor2;
    can_send_data_1ff[4] = (motor3 >> 8);
    can_send_data_1ff[5] = motor3;
    can_send_data_1ff[6] = (motor4 >> 8);
    can_send_data_1ff[7] = motor4;
    HAL_CAN_AddTxMessage(hcan, &tx_meessage_1ff, can_send_data_1ff, &send_mail_box);
}

/**
  * @brief          发送电机控制电流(0x201,0x202,0x203,0x204)
  * @param[in]      motor1: (0x201) 电机控制电流
  * @param[in]      motor2: (0x202) 电机控制电流
  * @param[in]      motor3: (0x203) 电机控制电流
  * @param[in]      motor4: (0x204) 电机控制电流
  * @retval         none
  */
void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_message_200.StdId = 0x200;
    tx_message_200.IDE = CAN_ID_STD;
    tx_message_200.RTR = CAN_RTR_DATA;
    tx_message_200.DLC = 0x08;
    can_send_data_200[0] = motor1 >> 8;
    can_send_data_200[1] = motor1;
    can_send_data_200[2] = motor2 >> 8;
    can_send_data_200[3] = motor2;
    can_send_data_200[4] = motor3 >> 8;
    can_send_data_200[5] = motor3;
    can_send_data_200[6] = motor4 >> 8;
    can_send_data_200[7] = motor4;

    HAL_CAN_AddTxMessage(hcan, &tx_message_200, can_send_data_200, &send_mail_box);
}

/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      motor1: (0x209) 电机控制电流
  * @param[in]      motor2: (0x20A) 电机控制电流
  * @param[in]      motor3: (0x20B) 电机控制电流
  * @retval         none
  */
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_meessage_2ff.StdId = 0x1FF;
    tx_meessage_2ff.IDE = CAN_ID_STD;
    tx_meessage_2ff.RTR = CAN_RTR_DATA;
    tx_meessage_2ff.DLC = 0x08;
    can_send_data_2ff[0] = (motor1 >> 8);
    can_send_data_2ff[1] = motor1;
    can_send_data_2ff[2] = (motor2 >> 8);
    can_send_data_2ff[3] = motor2;
    can_send_data_2ff[4] = (motor3 >> 8);
    can_send_data_2ff[5] = motor3;
    can_send_data_2ff[6] = (0 >> 8);
    can_send_data_2ff[7] = 0;
    HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
}

/**
  * @brief          返回电机数据指针
  * @param[in]      i: 电机编号
  * @retval         电机数据指针
  */
 motor_measure_t *get_motor_point(uint8_t i)
 {
     return &motor_chassis[i];
 }


/**
 * @brief          限制vesc电机转速
 * @param[in/out]  rpm:  vesce电机转速
 * @retval         none
 *
 * 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
 * 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
 */
void assert_param_rpm(float *rpm){
	if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
		*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}

/**
  * @brief          使vesc电机进入转速模式
  * @param[in]      controller_id: vesc电机控制器id
  * @param[in]      RPM: 电机转速
  * @retval         RPM(1000-50000之间的数)
  */
void  CAN_VESC_RPM(uint8_t controller_id, float RPM)
{
			uint32_t id;
			int32_t data;
			uint32_t send_mail_box;
	
			id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
			assert_param_rpm(&RPM);
			data = (int32_t)(RPM);
	
			vesc_tx_message.ExtId = id;
			vesc_tx_message.IDE = CAN_ID_EXT;
			vesc_tx_message.RTR = CAN_RTR_DATA;
			vesc_tx_message.DLC = 0x04;
			vesc_send_data[0] = data >> 24 ;
			vesc_send_data[1] = data >> 16 ;
			vesc_send_data[2] = data >> 8 ;
			vesc_send_data[3] = data ;
	
			HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
}


/**
  * @brief          使vesc电机进入制动模式
  * @param[in]      controller_id: vesc电机控制器id
  * @retval         none
  */
void  CAN_VESC_HEAD(uint8_t controller_id)
{
			uint32_t id;
			uint32_t send_mail_box;
	
			id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
	
			vesc_tx_message.ExtId = id;
			vesc_tx_message.IDE = CAN_ID_EXT;
			vesc_tx_message.RTR = CAN_RTR_DATA;
			vesc_tx_message.DLC = 0x04;
	
			HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
}