You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
426 lines
15 KiB
426 lines
15 KiB
#include "zcanreceiver.hpp"
|
|
|
|
#include "stm32basic/mutex.hpp"
|
|
#include "stm32halport/stm32halport.hpp"
|
|
#define TAG "zcan"
|
|
#define CANHANDLER &hcan1
|
|
#define CAN_FILTER_INDEX 0
|
|
#define CAN_MAX_FILTER_NUM 7
|
|
#define CAN_FIFO_NUM CAN_RX_FIFO0
|
|
#define OVER_TIME_MS 30
|
|
#define HEART_OVERTIME (30 * 1000)
|
|
|
|
using namespace iflytop;
|
|
|
|
static uint8_t m_deviceId;
|
|
static zcanbus_on_rx_t m_rxlistener[30];
|
|
static uint32_t m_numListener = 0;
|
|
static zcanbus_on_connected_t m_connectedlistener;
|
|
|
|
static canrxbuffer_t m_rxbufcache;
|
|
|
|
static uint16_t reportIndex;
|
|
|
|
static uint8_t rxdata[200];
|
|
static uint8_t txbuff[200];
|
|
|
|
static uint32_t lastpacket_ticket;
|
|
static bool m_is_connected;
|
|
|
|
static uint8_t m_priority;
|
|
|
|
zmutex m_lock;
|
|
|
|
static void _oncanpacket(CAN_HandleTypeDef *hcan);
|
|
static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData);
|
|
static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/);
|
|
|
|
extern "C" {
|
|
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0CompleteCallback"); }
|
|
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1CompleteCallback"); }
|
|
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2CompleteCallback"); }
|
|
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0AbortCallback"); }
|
|
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1AbortCallback"); }
|
|
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2AbortCallback"); }
|
|
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { _oncanpacket(hcan); }
|
|
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo0FullCallback"); }
|
|
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1MsgPendingCallback"); }
|
|
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1FullCallback"); }
|
|
void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_SleepCallback"); }
|
|
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_WakeUpFromRxMsgCallback"); }
|
|
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_ErrorCallback"); }
|
|
}
|
|
|
|
/***********************************************************************************************************************
|
|
* ��消�处� *
|
|
***********************************************************************************************************************/
|
|
|
|
static void _oncanpacket(CAN_HandleTypeDef *hcan) {
|
|
if (CANHANDLER != hcan) return;
|
|
CAN_RxHeaderTypeDef pHeader;
|
|
uint8_t aData[8] /*8byte table*/;
|
|
while (_getRxMessage(&pHeader, aData)) {
|
|
_processOneCanPacket(&pHeader, aData);
|
|
}
|
|
}
|
|
|
|
static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/) {
|
|
/**
|
|
* @brief 读å�–当å‰�FIFOä¸?缓å˜äº†å?šå°‘帧的数æ�®
|
|
*/
|
|
uint32_t level = HAL_CAN_GetRxFifoFillLevel(CANHANDLER, CAN_FIFO_NUM);
|
|
if (level == 0) {
|
|
return false;
|
|
}
|
|
HAL_StatusTypeDef HAL_RetVal;
|
|
HAL_RetVal = HAL_CAN_GetRxMessage(CANHANDLER, CAN_FIFO_NUM, pHeader, aData);
|
|
if (HAL_OK == HAL_RetVal) {
|
|
// 处�接收到的can总线数�
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData) {
|
|
/**
|
|
* @brief 消æ�¯æ ¼å¼� 12bit from,1bit emergency
|
|
*
|
|
* [1] [4bit] [8bit] [8bit] [4bit/4bit]
|
|
* , from to frameNum/frameId
|
|
*/
|
|
uint8_t from = (pHeader->ExtId & 0x00FF0000) >> 16;
|
|
uint8_t to = (pHeader->ExtId & 0x0000FF00) >> 8;
|
|
uint8_t nframe = (pHeader->ExtId & 0x000000F0) >> 4;
|
|
uint8_t frameId = (pHeader->ExtId & 0x0000000F);
|
|
|
|
// ZLOGI(TAG, "from:%d to:%d nframe:%d frameId:%d", from, to, nframe, frameId);
|
|
|
|
if (pHeader->IDE == CAN_ID_STD) {
|
|
return;
|
|
}
|
|
|
|
// �接收主机消�
|
|
if (from != 1) {
|
|
return;
|
|
}
|
|
|
|
if (to != m_deviceId && to != 0xff) {
|
|
return;
|
|
}
|
|
|
|
// 上�接收到的消�还没有�的急��
|
|
if (m_rxbufcache.dataIsReady) {
|
|
ZLOGI(TAG, "discard rx packet ,last packet not processed");
|
|
return;
|
|
}
|
|
|
|
if (frameId == 0) {
|
|
m_rxbufcache.canPacketNum = 0;
|
|
}
|
|
|
|
if (frameId != m_rxbufcache.canPacketNum) {
|
|
m_rxbufcache.canPacketNum = 0;
|
|
ZLOGI(TAG, "discard rx packet ,due to lost packet");
|
|
return;
|
|
}
|
|
|
|
if (m_rxbufcache.canPacketNum < ZARRAY_SIZE(m_rxbufcache.canPacket)) {
|
|
if (m_rxbufcache.canPacketNum == 0) {
|
|
m_rxbufcache.header = *pHeader;
|
|
m_rxbufcache.from = from;
|
|
m_rxbufcache.to = to;
|
|
}
|
|
m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].datalen = pHeader->DLC;
|
|
memcpy(m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].aData, aData, 8);
|
|
m_rxbufcache.canPacketNum++;
|
|
} else {
|
|
ZLOGI(TAG, "discard rx packet ,due to buffer full");
|
|
m_rxbufcache.canPacketNum = 0;
|
|
return;
|
|
}
|
|
|
|
if (nframe == frameId + 1) {
|
|
if (m_rxbufcache.canPacketNum != nframe) {
|
|
m_rxbufcache.canPacketNum = 0;
|
|
ZLOGI(TAG, "discard rx packet ,due to lost packet");
|
|
return;
|
|
} else {
|
|
m_rxbufcache.dataIsReady = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
/***********************************************************************************************************************
|
|
* FUNC *
|
|
***********************************************************************************************************************/
|
|
|
|
static HAL_StatusTypeDef activateRxIT() {
|
|
HAL_StatusTypeDef hal_status = HAL_ERROR;
|
|
if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
|
|
hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
|
|
} else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
|
|
hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
|
|
} else {
|
|
ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
|
|
return hal_status;
|
|
}
|
|
return hal_status;
|
|
}
|
|
// static HAL_StatusTypeDef deactivateRxIT() {
|
|
// HAL_StatusTypeDef hal_status = HAL_ERROR;
|
|
// if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
|
|
// hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
|
|
// } else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
|
|
// hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
|
|
// } else {
|
|
// ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
|
|
// return hal_status;
|
|
// }
|
|
// return hal_status;
|
|
// }
|
|
|
|
HAL_StatusTypeDef initializeFilter() {
|
|
HAL_StatusTypeDef HAL_Status;
|
|
CAN_FilterTypeDef sFilterConfig;
|
|
|
|
uint32_t filterId;
|
|
uint32_t mask;
|
|
|
|
memset(&sFilterConfig, 0, sizeof(sFilterConfig));
|
|
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; // 设为MASK模�
|
|
sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; // CAN_FILTERSCALE_16BIT
|
|
sFilterConfig.FilterFIFOAssignment = CAN_FIFO_NUM; // 关�过滤器到rxfifoNum
|
|
sFilterConfig.FilterActivation = ENABLE; // 激活过滤器
|
|
sFilterConfig.SlaveStartFilterBank = CAN_MAX_FILTER_NUM; // slave filter start index
|
|
|
|
/*******************************************************************************
|
|
* 接收所有消� *
|
|
*******************************************************************************/
|
|
filterId = (0); //
|
|
mask = (0); //
|
|
sFilterConfig.FilterBank = CAN_FILTER_INDEX; //
|
|
sFilterConfig.FilterMaskIdLow = mask & 0xffff; //
|
|
sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16; //
|
|
sFilterConfig.FilterIdLow = filterId & 0xffff; //
|
|
sFilterConfig.FilterIdHigh = (filterId & 0xffff0000) >> 16; //
|
|
HAL_Status = HAL_CAN_ConfigFilter(CANHANDLER, &sFilterConfig);
|
|
if (HAL_Status != HAL_OK) {
|
|
ZLOGE(TAG, "HAL_CAN_ConfigFilter filter0 fail");
|
|
return HAL_Status;
|
|
}
|
|
// ZLOGI(TAG, "HAL_CAN_ConfigFilter filterID1 %08x", filterId >> 3);
|
|
return HAL_Status;
|
|
}
|
|
|
|
void zcanbus_init(uint8_t deviceId) {
|
|
m_deviceId = deviceId;
|
|
|
|
HAL_StatusTypeDef hal_status;
|
|
|
|
m_rxbufcache.dataIsReady = false;
|
|
m_rxbufcache.id = 0; // �接收�自主机的消�
|
|
m_rxbufcache.canPacketNum = 0;
|
|
|
|
hal_status = initializeFilter();
|
|
if (hal_status != HAL_OK) {
|
|
ZLOGE(TAG, "start can initializeFilter fail\r\n");
|
|
return;
|
|
}
|
|
|
|
hal_status = HAL_CAN_Start(CANHANDLER); // 开�CAN
|
|
if (hal_status != HAL_OK) {
|
|
ZLOGE(TAG, "start can fail\r\n");
|
|
return;
|
|
}
|
|
m_lock.init();
|
|
|
|
HAL_StatusTypeDef status = activateRxIT();
|
|
if (status != HAL_OK) {
|
|
ZLOGE(TAG, "activateRxIT fail\r\n");
|
|
ZASSERT(0);
|
|
return;
|
|
}
|
|
ZLOGI(TAG, "zcanbus init done");
|
|
}
|
|
void zcanbus_reglistener(zcanbus_on_rx_t rxlistener) {
|
|
ZASSERT(m_numListener < ZARRAY_SIZE(m_rxlistener));
|
|
m_rxlistener[m_numListener++] = rxlistener;
|
|
}
|
|
|
|
void zcanbus_reg_on_connected_listener(zcanbus_on_connected_t connectedlistener) { m_connectedlistener = connectedlistener; }
|
|
|
|
bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len) { return zcanbus_send_packet(to, packet, len, OVER_TIME_MS); }
|
|
|
|
static char *hex2str(uint8_t *data, size_t len) {
|
|
static char buf[200];
|
|
memset(buf, 0, sizeof(buf));
|
|
for (size_t i = 0; i < len; i++) {
|
|
sprintf(buf + i * 2, "%02x", data[i]);
|
|
}
|
|
return buf;
|
|
}
|
|
|
|
bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len, int overtimems) {
|
|
ZLOGI(TAG, "sendPacket to:%d, %s(%d)", to, hex2str(packet, len), len);
|
|
int npacket = len / 8 + (len % 8 == 0 ? 0 : 1);
|
|
if (npacket > 255) {
|
|
ZLOGE(TAG, "sendPacket fail, len:%d", len);
|
|
return false;
|
|
}
|
|
int finalpacketlen = len % 8 == 0 ? 8 : len % 8;
|
|
|
|
for (uint8_t i = 0; i < npacket; i++) {
|
|
bool suc = false;
|
|
if (i == npacket - 1) {
|
|
suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, finalpacketlen, overtimems);
|
|
} else {
|
|
suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, 8, overtimems);
|
|
}
|
|
if (!suc) {
|
|
// ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i);
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// static const char *canpacket_dump(uint8_t *data, int size) {
|
|
// static char buf[20];
|
|
// memset(buf, 0, sizeof(buf));
|
|
// for (int i = 0; i < size; i++) {
|
|
// sprintf(buf + i * 2, "%02x", data[i]);
|
|
// }
|
|
// return buf;
|
|
// }
|
|
|
|
bool zcanbus_send_sub_packet(uint8_t to, int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) {
|
|
CAN_TxHeaderTypeDef pHeader;
|
|
uint8_t aData[8] /*8byte table*/;
|
|
uint32_t txMailBox = 0;
|
|
|
|
uint32_t enterticket = zget_ticket();
|
|
|
|
memset(&pHeader, 0, sizeof(pHeader));
|
|
memset(aData, 0, sizeof(aData));
|
|
pHeader.StdId = 0x00;
|
|
pHeader.ExtId = (m_deviceId << 16) | (to << 8) | (npacket << 4) | (packetIndex);
|
|
pHeader.ExtId |= (m_priority & 0x0f) << 24;
|
|
pHeader.ExtId |= (0x01 << 28);
|
|
pHeader.IDE = CAN_ID_EXT;
|
|
pHeader.RTR = CAN_RTR_DATA;
|
|
pHeader.DLC = len;
|
|
pHeader.TransmitGlobalTime = DISABLE;
|
|
|
|
memcpy(aData, packet, len);
|
|
// ZLOGI(TAG, "tx %s", canpacket_dump(aData, len));
|
|
|
|
HAL_StatusTypeDef lastTransmitStatus = HAL_CAN_AddTxMessage(CANHANDLER, &pHeader, aData, &txMailBox);
|
|
if (lastTransmitStatus != HAL_OK) {
|
|
ZLOGE(TAG, "HAL_CAN_AddTxMessage fail");
|
|
return false;
|
|
}
|
|
|
|
while (HAL_CAN_IsTxMessagePending(CANHANDLER, txMailBox)) {
|
|
if (zhas_passedms(enterticket) > (uint32_t)overtimems) {
|
|
lastTransmitStatus = HAL_TIMEOUT;
|
|
HAL_CAN_AbortTxRequest(CANHANDLER, txMailBox);
|
|
return false;
|
|
}
|
|
zos_delay_ms(1);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, uint8_t *param, size_t len) {
|
|
zlock_guard l(m_lock);
|
|
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
|
|
txpacket->index = rxpacket->index;
|
|
txpacket->function_id = rxpacket->function_id;
|
|
txpacket->ptype = kreceipt;
|
|
if (param) memcpy(txpacket->params, param, len);
|
|
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len);
|
|
}
|
|
|
|
bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, int32_t val) { return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val)); }
|
|
bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, bool _val) {
|
|
int32_t val = _val ? 1 : 0;
|
|
return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val));
|
|
}
|
|
|
|
bool zcanbus_send_errorack(zcanbus_packet_t *rxpacket, int32_t errcode) {
|
|
zlock_guard l(m_lock);
|
|
m_priority = kpriority_receipt;
|
|
|
|
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
|
|
txpacket->index = rxpacket->index;
|
|
txpacket->function_id = rxpacket->function_id;
|
|
txpacket->ptype = kerror_receipt;
|
|
memcpy(txpacket->params, &errcode, sizeof(errcode));
|
|
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + 4);
|
|
}
|
|
bool zcanbus_send_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
|
|
zlock_guard l(m_lock);
|
|
m_priority = kpriority_report;
|
|
|
|
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
|
|
txpacket->index = reportIndex++;
|
|
txpacket->function_id = function_id;
|
|
txpacket->ptype = kreport;
|
|
memcpy(txpacket->params, param, len);
|
|
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
|
|
}
|
|
|
|
bool zcanbus_send_emergency_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
|
|
zlock_guard l(m_lock);
|
|
m_priority = kpriority_emergency_report;
|
|
|
|
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
|
|
txpacket->index = reportIndex++;
|
|
txpacket->function_id = function_id;
|
|
txpacket->ptype = kreport;
|
|
memcpy(txpacket->params, param, len);
|
|
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
|
|
}
|
|
|
|
static void process_rx_packet(canrxbuffer_t *canrxbuf, uint8_t *rx, size_t len) {
|
|
zcanbus_packet_t *packet = (zcanbus_packet_t *)rx;
|
|
|
|
lastpacket_ticket = zget_ticket();
|
|
if (!m_is_connected) {
|
|
m_is_connected = true;
|
|
if (m_connectedlistener) m_connectedlistener(true);
|
|
}
|
|
|
|
if (packet->ptype == kcmd) {
|
|
for (size_t i = 0; i < m_numListener; i++) {
|
|
m_rxlistener[i](canrxbuf->from, canrxbuf->to, rxdata, len);
|
|
}
|
|
}
|
|
}
|
|
|
|
void zcanbus_schedule() {
|
|
canrxbuffer_t *rxbuf = &m_rxbufcache;
|
|
|
|
uint16_t fromId = 0;
|
|
uint16_t toId = 0;
|
|
|
|
if (rxbuf->dataIsReady) {
|
|
int dataoff = 0;
|
|
// rxdata[0] = rxbuf->from;
|
|
// rxdata[1] = rxbuf->to;
|
|
for (size_t i = 0; i < rxbuf->canPacketNum; i++) {
|
|
memcpy(rxdata + dataoff, rxbuf->canPacket[i].aData, rxbuf->canPacket[i].datalen);
|
|
dataoff += rxbuf->canPacket[i].datalen;
|
|
ZASSERT(dataoff < ZARRAY_SIZE(rxdata));
|
|
}
|
|
process_rx_packet(rxbuf, rxdata, dataoff);
|
|
rxbuf->dataIsReady = false;
|
|
}
|
|
|
|
if (m_is_connected && zhas_passedms(lastpacket_ticket) > HEART_OVERTIME) {
|
|
m_is_connected = false;
|
|
if (m_connectedlistener) m_connectedlistener(false);
|
|
}
|
|
}
|
|
|
|
bool zcanbus_is_connected() { return m_is_connected; }
|