#include "odrive_can.h"
#include "can_it.h"
#include "main.h"
#include "can.h"
/******小端模式，低字节在前***/
/*********数据拆分************/
#define BYTE0(dwTemp)       (*(char *)(&dwTemp))	 
#define BYTE1(dwTemp)       (*((char *)(&dwTemp) + 1))	 
#define BYTE2(dwTemp)       (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp)       (*((char *)(&dwTemp) + 3))

//请在此处写明你所使用的can是can1还是can2
#define WHICH_CAN 2
//如果已经在其他地方完成了can初始化，则此处不进行can初始化
#define INITED 1

 odrive_msg_t odrive_msg;

/**
 * 用户自定义can1中断回调函数
*/

void User_Odrive_Can1_CB(void)
{
    CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8];

	HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &rx_header, rx_data);
	switch (rx_header.StdId)
    {
        case (AXIS0_NODE << 5) | ODRIVE_ENCODER_VALUE:
            odrive_msg.pos = rx_data[3] << 24 | rx_data[2] << 16 | rx_data[1] << 8 | rx_data[0];
            odrive_msg.speed = rx_data[7] << 24 | rx_data[6] << 16 | rx_data[5] << 8 | rx_data[4];
            break;
    }
}
#if WHICH_CAN == 2
/**
 * 用户自定义can2中断回调函数
*/
	uint8_t rx_data[8];
void User_Odrive_Can2_CB(void)
{
    CAN_RxHeaderTypeDef rx_header;


	HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
	switch (rx_header.StdId)
    {
        case (AXIS0_NODE << 5) | ODRIVE_ENCODER_VALUE:
            odrive_msg.pos = rx_data[3] << 24 | rx_data[2] << 16 | rx_data[1] << 8 | rx_data[0];
            odrive_msg.speed = rx_data[7] << 24 | rx_data[6] << 16 | rx_data[5] << 8 | rx_data[4];
            break;
    }
}
#endif


/**
 * @brief  odrive初始化
 * @param  none
 * @return  device_status_e，若值为0，则为成功
 */
device_status_e Odrive_Can_Init(void)
{
#if INITED == 0
/**********************can初始化*********************/
    CAN_FilterTypeDef can_filter_st;
    can_filter_st.FilterActivation = ENABLE;
    can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
    can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
    can_filter_st.FilterIdHigh = 0x0000;
    can_filter_st.FilterIdLow = 0x0000;
    can_filter_st.FilterMaskIdHigh = 0x0000;
    can_filter_st.FilterMaskIdLow = 0x0000;

    can_filter_st.FilterBank = 0;
    can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
    HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
    HAL_CAN_Start(&hcan1);
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
 #if WHICH_CAN == 1
    //指向自定义的中断回调函数
    BSP_CAN_RegisterCallback(BSP_CAN_1,HAL_CAN_RX_FIFO0_MSG_PENDING_CB,&User_Odrive_Can1_CB);
#endif
#if WHICH_CAN == 2
//开启can2过滤器之前必须先开启can1
  
    can_filter_st.SlaveStartFilterBank = 14;
    can_filter_st.FilterBank = 14;
    can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
    HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
    HAL_CAN_Start(&hcan2);
    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
    //指向自定义的中断回调函数
    BSP_CAN_RegisterCallback(BSP_CAN_2,HAL_CAN_RX_FIFO1_MSG_PENDING_CB,&User_Odrive_Can2_CB);

#endif

#else
#if WHICH_CAN == 1
//若已经在别处初始化can，则此处只进行指向自定义的中断回调函数
     BSP_CAN_RegisterCallback(BSP_CAN_1,HAL_CAN_RX_FIFO0_MSG_PENDING_CB,&User_Odrive_Can1_CB);
#else

    BSP_CAN_RegisterCallback(BSP_CAN_2,HAL_CAN_RX_FIFO1_MSG_PENDING_CB,&User_Odrive_Can2_CB);
#endif
#endif
    return DEVICE_OK;
}

// void odrive_CB(void)
// {
//   BSP_CAN_RegisterCallback(BSP_CAN_1,HAL_CAN_RX_FIFO0_MSG_PENDING_CB,&User_Odrive_Can1_CB);

// }//我自己加的

static CAN_TxHeaderTypeDef  tx_message;
static uint8_t              can_send_data[8];

/**
 * @brief  速度模式控制电机
 * @param  node_id 节点id，AXIS0_NODE或者AXIS0_NODE
 * @param  speed 目标速度
 * @return  device_status_e，若值为0，则为成功
 */
device_status_e odrive_speed_cmd(node_id_e node_id, fp32 speed)
{
    if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
    {
        return DEVICE_ERR;
    }//如果节点id错误，则返回错误


    uint32_t send_mail_box;
    tx_message.StdId = node_id << 5 | ODRIVE_SPEED_SET;
    tx_message.IDE = CAN_ID_STD;
    tx_message.RTR = CAN_RTR_DATA;
    tx_message.DLC = 0x08;
    can_send_data[0] = BYTE0(speed);
    can_send_data[1] = BYTE1(speed);
    can_send_data[2] = BYTE2(speed);
    can_send_data[3] = BYTE3(speed);
    can_send_data[4] = 0;
    can_send_data[5] = 0;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
#if WHICH_CAN == 1
    HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
#else
    HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
#endif
    return DEVICE_OK;
}
uint8_t a;
/**
 * @brief  位置模式控制电机
 * @param  node_id 节点id，AXIS0_NODE或者AXIS0_NODE
 * @param  pos 目标位置
 * @param  accel 加速度限制
 * @param  decel 减速度限制
 * @return  device_status_e，若值为0，则为成功
 */
device_status_e odrive_pos_cmd(node_id_e node_id, fp32 pos)
{
    if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
    {
        return DEVICE_ERR;
    }//如果节点id错误，则返回错误

    uint32_t send_mail_box;
    //发送位置
    tx_message.StdId = node_id << 5 | ODRIVE_POS_SET;
    tx_message.IDE = CAN_ID_STD;
    tx_message.RTR = CAN_RTR_DATA;
    tx_message.DLC = 0x08;
    can_send_data[0] = BYTE0(pos);
    can_send_data[1] = BYTE1(pos);
    can_send_data[2] = BYTE2(pos);
    can_send_data[3] = BYTE3(pos);
    can_send_data[4] = 0;
    can_send_data[5] = 0;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
	

#if WHICH_CAN == 1
   a= HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
#else
    HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
#endif
    return DEVICE_OK;
}

/**
 * @brief  位置模式控制电机
 * @param  node_id 节点id，AXIS0_NODE或者AXIS0_NODE
 * @param  pos 目标位置
 * @param  accel 加速度限制
 * @param  decel 减速度限制
 * @return  device_status_e，若值为0，则为成功
 */
device_status_e odrive_accel_cmd(node_id_e node_id, fp32 accel , fp32 decel)
{
	  if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
    {
      return DEVICE_ERR;
    }//如果节点id错误，则返回错误

    //发送加减速
    uint32_t send_mail_box;
    tx_message.StdId = node_id << 5 | ODRIVE_TRAJ_ACC_LIM;
    tx_message.IDE = CAN_ID_STD;
    tx_message.RTR = CAN_RTR_DATA;
    tx_message.DLC = 0x08;
    can_send_data[0] = BYTE0(accel);
    can_send_data[1] = BYTE1(accel);
    can_send_data[2] = BYTE2(accel);
    can_send_data[3] = BYTE3(accel);
    can_send_data[4] = BYTE0(decel);
    can_send_data[5] = BYTE1(decel);
    can_send_data[6] = BYTE2(decel);
    can_send_data[7] = BYTE3(decel);
#if WHICH_CAN == 1
    HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
#else
    HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
#endif
	return DEVICE_OK;
}



/**
 * @brief  获取编码器估算值
 * @param  node_id 节点id，AXIS0_NODE或者AXIS0_NODE
 * @return  device_status_e，若值为0，则为成功
 */
device_status_e odrive_get_encode(node_id_e node_id)
{
    if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
    {
        return DEVICE_ERR;
    }//如果节点id错误，则返回错误


    uint32_t send_mail_box;
    tx_message.StdId = node_id << 5 | ODRIVE_ENCODER_VALUE;
    tx_message.IDE = CAN_ID_STD;
    tx_message.RTR = CAN_RTR_REMOTE;
    tx_message.DLC = 0x08;
    can_send_data[0] = 0x10;
    can_send_data[1] = 0;
    can_send_data[2] = 0;
    can_send_data[3] = 0;
    can_send_data[4] = 0;
    can_send_data[5] = 0;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
#if WHICH_CAN == 1
    HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
#else
    HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
#endif
    return DEVICE_OK;
}


/**
 * @brief  获取odrive结构体指针
 * @param  none
 * @return  信息结构体指针
 */
odrive_msg_t* Get_Odrive_Point(void)
{
	return &odrive_msg;
}



