357 lines
12 KiB
C
357 lines
12 KiB
C
/*
|
||
CAN总线上的讄1<E8AE84>7
|
||
将所有CAN总线上挂载的设抽象成丢<E68890>设进行配和控刄1<E58884>7
|
||
*/
|
||
|
||
|
||
/* 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) /* 电机转矩电流分辨玄1<E78E84>7 */
|
||
|
||
|
||
#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)
|
||
|
||
/* 电机朢<E69CBA>大电流绝对<E7BB9D><E5AFB9>1<EFBFBD>7 */
|
||
#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;
|
||
static CAN_RawRx_t raw_rx2;
|
||
CAN_RawTx_t raw_tx;
|
||
|
||
static CAN_t *gcan;//有什么用＄1<EFBC84>7
|
||
|
||
/* Private function -------------------------------------------------------- */
|
||
|
||
static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
|
||
const uint8_t *raw) {
|
||
if (feedback == NULL || raw == NULL) return;
|
||
uint16_t raw_angle = (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_angle = raw_angle / (float)CAN_MOTOR_ENC_RES * 360.0f;
|
||
feedback->torque_current =
|
||
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
|
||
}
|
||
|
||
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;
|
||
}
|
||
//接收四个sick板的数据
|
||
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,const uint8_t *raw) {
|
||
if (feedback == NULL || raw == NULL) return;
|
||
|
||
feedback->sick_distance = (uint16_t)(raw[0] << 8| raw[1]);
|
||
}
|
||
//接收一个sick_mini板的数据
|
||
static void CAN_Sick_Receive_mini(CAN_SickFeedback_mini_t *feedback,const uint8_t *raw) {
|
||
if (feedback == NULL || raw == NULL) return;
|
||
feedback->sick_distance[0] = (uint16_t)(raw[0] << 8| raw[1]);
|
||
feedback->sick_distance[1] = (uint16_t)(raw[2] << 8| raw[3]);
|
||
feedback->sick_distance[2] = (uint16_t)(raw[4] << 8| raw[5]);
|
||
feedback->sick_distance[3] = (uint16_t)(raw[6] << 8| raw[7]);
|
||
|
||
}
|
||
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);
|
||
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);
|
||
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.chassis));
|
||
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.chassis));
|
||
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.chassis));
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
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;
|
||
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.chassis));
|
||
}
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
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;
|
||
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);
|
||
detect_hook(CHASSIS6020M1_TOE+index);
|
||
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;
|
||
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;
|
||
//接收四个sick小板数据
|
||
// case CAN_SICK_ID1:
|
||
// case CAN_SICK_ID2:
|
||
// case CAN_SICK_ID3:
|
||
// case CAN_SICK_ID4:
|
||
// index = can_rx->rx_header.StdId - CAN_SICK_ID1;
|
||
// can->recive_flag |= 1 << (index);
|
||
// CAN_Sick_Receive(&(can->sickfed.as_array[index]), can_rx->rx_data);
|
||
// break;
|
||
//接收sick mini数据
|
||
case CAN_SICK_ID0:
|
||
CAN_Sick_Receive_mini(&(can->sicked_mini), can_rx->rx_data);
|
||
break;
|
||
default:
|
||
break;
|
||
|
||
}
|
||
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);
|
||
detect_hook(CHASSIS5065M1_TOE);
|
||
break;
|
||
case CAN_VESC5065_M2_MSG1:
|
||
CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[1]), can_rx->rx_data);
|
||
detect_hook(CHASSIS5065M2_TOE);
|
||
break;
|
||
case CAN_VESC5065_M3_MSG1:
|
||
CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[2]), can_rx->rx_data);
|
||
detect_hook(CHASSIS5065M3_TOE);
|
||
break;
|
||
case CAN_VSEC5065_M4_MSG1:
|
||
CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[3]), can_rx->rx_data);
|
||
detect_hook(CHASSIS5065M4_TOE);
|
||
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;
|
||
}
|