401 lines
13 KiB
C
401 lines
13 KiB
C
/*
|
||
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;
|
||
}
|