/* CAN总线上的设备1到7 将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制 */ /* Includes ----------------------------------------------------------------- */ #include "can_use.h" #include "user_math.h" #include "device.h" #include "error_detect.h" #define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */ #define CAN_MOTOR_CUR_RES (16384) #define CAN_GM6020_CTRL_ID_BASE (0x1ff) #define CAN_PITCH6020_CTRL_ID (0x2ff) #define CAN_G3508_CTRL_ALL_ID (0x200) #define CAN_VESC_CTRL_ID_BASE (0x300) #define CAN_MOTOR_TX_BUF_SIZE (8) #define CAN_MOTOR_RX_BUF_SIZE (8) /*电机最大电流绝对值*/ #define CAN_GM6020_MAX_ABS_CUR (20) #define CAN_M3508_MAX_ABS_CUR (20) #define CAN_M2006_MAX_ABS_CUR (10) static CAN_RawRx_t raw_rx1;//原始的can数据 static CAN_RawRx_t raw_rx2; CAN_RawTx_t raw_tx; /*用于can原始数据，传入消息队列*/ static CAN_t *gcan; /* Private function -------------------------------------------------------- */ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback, const uint8_t *raw) { if (feedback == NULL || raw == NULL) return; uint16_t raw_ecd = (uint16_t)((raw[0] << 8) | raw[1]); int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]); feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]); feedback->temp = raw[6]; feedback->rotor_ecd = raw_ecd ; feedback->torque_current = raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; } static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, const uint8_t *raw) { if (feedback == NULL || raw == NULL) return; feedback->raw_dis[0] = (uint16_t)((raw[0] << 8) | raw[1]); feedback->raw_dis[1] = (uint16_t)((raw[2] << 8) | raw[3]); feedback->raw_dis[2] = (uint16_t)((raw[4] << 8) | raw[5]); feedback->raw_dis[3]= (uint16_t)((raw[6] << 8) | raw[7]); } 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; } static void CAN_CAN1RxFifoMsgPendingCallback(void) { HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_RX_FIFO0, &raw_rx1.rx_header, raw_rx1.rx_data); raw_rx1.hcan =BSP_CAN_GetHandle(BSP_CAN_1); osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0); } static void CAN_CAN2RxFifoMsgPendingCallback(void) { HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_2), CAN_RX_FIFO1, &raw_rx2.rx_header, raw_rx2.rx_data); raw_rx2.hcan =BSP_CAN_GetHandle(BSP_CAN_2); osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0); } /* Exported functions ------------------------------------------------------- */ int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) { if (can == NULL) return DEVICE_ERR_NULL; can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL); can->param = param; CAN_FilterTypeDef can_filter = {0}; can_filter.FilterBank = 0; can_filter.FilterIdHigh = 0; can_filter.FilterIdLow = 0; can_filter.FilterMode = CAN_FILTERMODE_IDMASK; can_filter.FilterScale = CAN_FILTERSCALE_32BIT; can_filter.FilterMaskIdHigh = 0; can_filter.FilterMaskIdLow = 0; can_filter.FilterActivation = ENABLE; can_filter.SlaveStartFilterBank = 14; can_filter.FilterFIFOAssignment = CAN_RX_FIFO0; HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_1), &can_filter); HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_1)); BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, CAN_CAN1RxFifoMsgPendingCallback); HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_1), CAN_IT_RX_FIFO0_MSG_PENDING); can_filter.FilterBank = 14; can_filter.FilterFIFOAssignment = CAN_RX_FIFO1; HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_2), &can_filter); HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_2)); BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, CAN_CAN2RxFifoMsgPendingCallback); HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_2), CAN_IT_RX_FIFO1_MSG_PENDING); gcan = can; return DEVICE_OK; } int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output, CAN_t *can) { if (output == NULL) return DEVICE_ERR_NULL; int16_t motor1, motor2, motor3, motor4, motor5 ; switch (group) { case CAN_MOTOR_CHASSIS6020: motor1 = (int16_t)(output->chassis6020.named.m1); motor2 = (int16_t)(output->chassis6020.named.m2); motor3 = (int16_t)(output->chassis6020.named.m3); motor4 = (int16_t)(output->chassis6020.named.m4); raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE; raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF); raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF); raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF); raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF); raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF); raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF); raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF); raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF); HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020), &raw_tx.tx_header, raw_tx.tx_data, &(can->mailbox.up_6020 )); break; case CAN_MOTOR_3508: motor1 = (int16_t)(output->motor3508.named.m1); motor2 = (int16_t)(output->motor3508.named.m2); motor3 = (int16_t)(output->motor3508.named.m3); motor4 = (int16_t)(output->motor3508.named.m4); raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_ID; raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF); raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF); raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF); raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF); raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF); raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF); raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF); raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF); HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor3508), &raw_tx.tx_header, raw_tx.tx_data, &(can->mailbox.up_3508 )); break; case CAN_MOTOR_PITCH6020: motor5 = (int16_t)(output->pitch6020.named.m1); raw_tx.tx_header.StdId = CAN_PITCH6020_CTRL_ID; raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_data[0] = (uint8_t)((motor5 >> 8) & 0xFF); raw_tx.tx_data[1] = (uint8_t)(motor5 & 0xFF); raw_tx.tx_data[2] = 0; raw_tx.tx_data[3] = 0; raw_tx.tx_data[4] = 0; raw_tx.tx_data[5] = 0; raw_tx.tx_data[6] = 0; raw_tx.tx_data[7] = 0; HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->pitch6020), &raw_tx.tx_header, raw_tx.tx_data, &(can->mailbox.up_6020 )); break; default: break; } return DEVICE_OK; } //用来问答接收来自sick的数据 void CAN_Sick_Control(CAN_t *can) { raw_tx.tx_header.StdId = 0x301; raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.DLC = 0x08; raw_tx.tx_header.RTR = CAN_RTR_DATA;//˽ߝ֡ raw_tx.tx_data[0] = 0x00; raw_tx.tx_data[1] = 0x00; raw_tx.tx_data[2] = 0x00; raw_tx.tx_data[3] = 0x00; raw_tx.tx_data[4] = 0x00; raw_tx.tx_data[5] = 0x00; raw_tx.tx_data[6] = 0x00; raw_tx.tx_data[7] = 0x00; HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->sick),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.up_other )); } int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){ if (output == NULL) return DEVICE_ERR_NULL; int Byte[4]; Vesc_ByteGet raw[4]; switch (group) { case CAN_MOTOR_CHASSIS5065: //将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节 raw[0].as_array = output->chassis5065.named.m1; raw[1].as_array = output->chassis5065.named.m2; raw[2].as_array = output->chassis5065.named.m3; raw[3].as_array = output->chassis5065.named.m4; for(int i=0 ; i < 4 ; i ++) { if(i==2) //此处可能同时发送四个vesc导致只有三个轮子接收到数据 故加上delay { osDelay(1); } //将单个电机的期望输出值通过联合体拆分 Byte[0] = raw[i].byte.byte1; Byte[1] = raw[i].byte.byte2; Byte[2] = raw[i].byte.byte3; Byte[3] = raw[i].byte.byte4; raw_tx.tx_header.ExtId = id+i+CAN_VESC_CTRL_ID_BASE; raw_tx.tx_header.IDE = CAN_ID_EXT; raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_data[0] = Byte[3]; raw_tx.tx_data[1] = Byte[2]; raw_tx.tx_data[2] = Byte[1]; raw_tx.tx_data[3] = Byte[0]; raw_tx.tx_data[4] = 0; raw_tx.tx_data[5] = 0; raw_tx.tx_data[6] = 0; raw_tx.tx_data[7] = 0; HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis5065), &raw_tx.tx_header, raw_tx.tx_data, &(can->mailbox.up_5065 )); } } return DEVICE_OK; } /** * @brief 将接收到的CAN消息存储在CAN_t结构体中，对容易有id冲突的进行can1和can2分类 * @param can 要存储消息的CAN_t结构体 * @param can_rx 接收到的CAN消息 * @return 如果消息存储成功，DEVICE_ERR_NULL 如果can或can_rx为NULL */ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) { // 检查输入参数是否为空 if (can == NULL) return DEVICE_ERR_NULL; if (can_rx == NULL) return DEVICE_ERR_NULL; uint32_t index; // 处理CAN1消息 if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_1)) { switch (can_rx->rx_header.StdId) { case CAN_G6020_AgvM1: case CAN_G6020_AgvM2: case CAN_G6020_AgvM3: case CAN_G6020_AgvM4: // 存储消息到对应的电机结构体中 index = can_rx->rx_header.StdId - CAN_G6020_AgvM1; can->recive_flag |= 1 << (index); CAN_DJIMotor_Decode(&(can->motor.chassis6020.as_array[index]), can_rx->rx_data); break; case CAN_M3508_M1_ID: case CAN_M3508_M2_ID: case CAN_M3508_M3_ID: case CAN_M3508_M4_ID: // 存储消息到对应的电机结构体中 index = can_rx->rx_header.StdId - CAN_M3508_M1_ID; can->recive_flag |= 1 << (index); CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data); break; default: break; } } // 处理CAN2消息 if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_2)) { switch (can_rx->rx_header.StdId) { case CAN_G6020_Pitch: // 存储消息到对应的电机结构体中 index = can_rx->rx_header.StdId - CAN_G6020_Pitch; can->recive_flag |= 1 << (index); CAN_DJIMotor_Decode(&(can->motor.pit6020.as_array[index]), can_rx->rx_data); break; case CAN_SICK_ID: // 存储消息到sickfed结构体中 CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data); break; default: break; } } // 处理扩展ID消息 switch (can_rx->rx_header.ExtId) { case CAN_VESC5065_M1_MSG1: // 存储消息到对应的电机结构体中 CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[0]), can_rx->rx_data); break; case CAN_VESC5065_M2_MSG1: // 存储消息到对应的电机结构体中 CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[1]), can_rx->rx_data); break; // case CAN_VESC5065_M3_MSG1: // // 存储消息到对应的电机结构体中 // CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[2]), can_rx->rx_data); // break; // case CAN_VSEC5065_M4_MSG1: // // 存储消息到对应的电机结构体中 // CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[3]), can_rx->rx_data); break; default: break; } return DEVICE_OK; } bool_t CAN_CheckFlag(CAN_t *can, uint32_t flag) { if (can == NULL) return false; return (can->recive_flag & flag) == flag; } int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag) { if (can == NULL) return DEVICE_ERR_NULL; can->recive_flag &= ~flag; return DEVICE_OK; }