/** ****************************(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 #include "FreeRTOS.h" #include "odrive_can.h" #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]; \ if ((ptr)->ecd - (ptr)->last_ecd > 4096) \ (ptr)->round_cnt--; \ else if ((ptr)->ecd - (ptr)->last_ecd < -4096) \ (ptr)->round_cnt++; \ if((ptr)->type == M3508) \ (ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508; \ else if((ptr)->type == M2006) \ (ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006; \ else \ (ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \ (ptr)->real_round = (ptr)->total_angle / 360; \ (ptr)->real_angle = (ptr)->total_angle % 360; \ } // #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; */ #define get_motor_offset(ptr, data) \ { \ (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \ (ptr)->offset_ecd = (ptr)->ecd; \ } /*上下3508为0;翻转3508为1;前后2006为2;夹球2006为3,取球2006为4*/ #if DEBUG == 1 motor_measure_t motor_chassis[5]; #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]; /** * @brief 数据处理函数 * @param[in] none * @retval none */ void djiMotorEncode() { 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 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]; }