362 lines
12 KiB
C
362 lines
12 KiB
C
/**
|
||
****************************(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"
|
||
#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; \
|
||
}
|
||
|
||
/*(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; \
|
||
}
|
||
|
||
void VescMotor_Decode(CAN_MotorFeedback_t *feedback, CAN_RxHeaderTypeDef *rx_header, const uint8_t *rx_data)
|
||
{
|
||
// 检查接收到的ID是否匹配我们期望的VESC状态消息ID
|
||
if (rx_header->ExtId == CAN_VESC5065_M1_MSG1) // 电机 1 的消息 ID
|
||
{
|
||
// 提取数据字段
|
||
feedback->rotor_speed = ((int32_t)(rx_data[0]) << 24) | ((int32_t)(rx_data[1]) << 16) | ((int32_t)(rx_data[2]) << 8) | ((int32_t)(rx_data[3]));
|
||
feedback->torque_current = ((int16_t)(rx_data[5]) << 8) | (int16_t)(rx_data[4]);
|
||
feedback->torque_current /= 1000; // 将单位从 0.1A 转换为实际电流值
|
||
feedback->duty_cycle = ((int16_t)(rx_data[7]) << 8) | (int16_t)(rx_data[6]);
|
||
feedback->duty_cycle /= 1000.0f; // 将单位从千分之一转换为实际的占空比值 (-1.0 到 1.0)
|
||
}
|
||
else if (rx_header->ExtId == CAN_VESC5065_M1_MSG1) // 电机 2 的消息 ID
|
||
{
|
||
// 提取数据字段
|
||
feedback->rotor_speed = ((int32_t)(rx_data[0]) << 24) | ((int32_t)(rx_data[1]) << 16) | ((int32_t)(rx_data[2]) << 8) | ((int32_t)(rx_data[3]));
|
||
feedback->torque_current = ((int16_t)(rx_data[5]) << 8) | (int16_t)(rx_data[4]);
|
||
feedback->torque_current /= 1000; // 将单位从 0.1A 转换为实际电流值
|
||
feedback->duty_cycle = ((int16_t)(rx_data[7]) << 8) | (int16_t)(rx_data[6]);
|
||
feedback->duty_cycle /= 1000.0f; // 将单位从千分之一转换为实际的占空比值 (-1.0 到 1.0)
|
||
}
|
||
}
|
||
#if DEBUG == 1
|
||
|
||
//电机回传数据结构体
|
||
motor_measure_t motor_chassis[10];
|
||
CAN_MotorFeedback_t motor_5065[2];
|
||
#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 rx_header;
|
||
uint8_t rx_data[8];
|
||
|
||
static CAN_TxHeaderTypeDef vesc_tx_message;
|
||
static uint8_t vesc_send_data[4];
|
||
/**
|
||
* @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;
|
||
}
|
||
}
|
||
}
|
||
|
||
void can2MotorEncode()
|
||
{
|
||
switch (rx_header.ExtId) {
|
||
|
||
case CAN_VESC5065_M1_MSG1:
|
||
// 存储消息到对应的电机结构体中
|
||
VescMotor_Decode(&motor_5065[0], &rx_header,rx_data);
|
||
break;
|
||
|
||
case CAN_VESC5065_M2_MSG1:
|
||
// 存储消息到对应的电机结构体中
|
||
VescMotor_Decode(&motor_5065[1], &rx_header,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);
|
||
}
|
||
void can2_Motor_CB(void)
|
||
{
|
||
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
|
||
}
|
||
|
||
/**
|
||
* @brief 大疆电机初始化
|
||
* @param none
|
||
* @retval none
|
||
*/
|
||
void djiInit(void)
|
||
{
|
||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
|
||
Dji_Motor_CB);
|
||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
|
||
can2_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 = 0x2FF;
|
||
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(&hcan2, &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(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
|
||
}
|