/** ****************************(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); }