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.
474 lines
15 KiB
474 lines
15 KiB
#include "zcanreceiver_master.hpp"
|
|
|
|
#include "project_configs.h"
|
|
#include "sdk\components\zprotocols\errorcode\errorcode.hpp"
|
|
#ifdef HAL_CAN_MODULE_ENABLED
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
using namespace iflytop;
|
|
using namespace zcr;
|
|
|
|
#define TAG "ZCanCommnaderMaster"
|
|
|
|
#define OVER_TIME_MS 5
|
|
|
|
#define MASK_32BIT(off) (0x01 << (off))
|
|
|
|
ZCanCommnaderMaster::CFG *ZCanCommnaderMaster::createCFG() {
|
|
CFG *cfg = new CFG();
|
|
ZASSERT(cfg != NULL);
|
|
cfg->deviceId = 1;
|
|
#ifdef STM32F103xB
|
|
cfg->canHandle = &hcan;
|
|
#else
|
|
cfg->canHandle = &hcan1;
|
|
#endif
|
|
cfg->canFilterIndex0 = 0;
|
|
cfg->maxFilterNum = 7;
|
|
cfg->rxfifoNum = CAN_RX_FIFO0;
|
|
return cfg;
|
|
}
|
|
void ZCanCommnaderMaster::init(CFG *cfg) {
|
|
HAL_StatusTypeDef hal_status;
|
|
m_config = cfg;
|
|
m_on_packet_map_lock.init();
|
|
txlock.init();
|
|
|
|
/**
|
|
* @brief 初始化CAN
|
|
*/
|
|
|
|
/**
|
|
* @brief 初始化消息接收buf
|
|
*/
|
|
m_canPacketRxBuffer[0].dataIsReady = false;
|
|
m_canPacketRxBuffer[0].id = 1; // 只接收来自主机的消息
|
|
m_canPacketRxBuffer[0].m_canPacketNum = 0;
|
|
|
|
/**
|
|
* @brief 初始化过滤器
|
|
*/
|
|
hal_status = initializeFilter();
|
|
if (hal_status != HAL_OK) {
|
|
ZLOGE(TAG, "start can initializeFilter fail\r\n");
|
|
return;
|
|
}
|
|
/**
|
|
* @brief 启动CAN
|
|
*/
|
|
hal_status = HAL_CAN_Start(m_config->canHandle); // 开启CAN
|
|
if (hal_status != HAL_OK) {
|
|
ZLOGE(TAG, "start can fail\r\n");
|
|
return;
|
|
}
|
|
/**
|
|
* @brief 监听回调
|
|
*/
|
|
ZCanIRQDispatcher::instance().regListener(this);
|
|
HAL_StatusTypeDef status = activateRxIT();
|
|
if (status != HAL_OK) {
|
|
ZLOGE(TAG, "activateRxIT fail\r\n");
|
|
return;
|
|
}
|
|
m_loopThread.init("ZCanCommnaderMaster", 1024, osPriorityAboveNormal);
|
|
m_loopThread.start([this]() {
|
|
while (true) {
|
|
loop();
|
|
osDelay(1);
|
|
}
|
|
});
|
|
}
|
|
HAL_StatusTypeDef ZCanCommnaderMaster::initializeFilter() {
|
|
/**
|
|
* @brief ID区帧格式
|
|
* [ 27:0 ]
|
|
* [ STDID ] [ EXTID ]
|
|
* [11 :9] [8:6] [5:0] [17:16] [15:8] [7:0]
|
|
* 优先级 属性 帧类型 目标ID 源ID
|
|
*/
|
|
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 = m_config->rxfifoNum; // 关联过滤器到rxfifoNum
|
|
sFilterConfig.FilterActivation = ENABLE; // 激活过滤器
|
|
sFilterConfig.SlaveStartFilterBank = m_config->maxFilterNum; // slave filter start index
|
|
|
|
/*******************************************************************************
|
|
* 接收所有消息 *
|
|
*******************************************************************************/
|
|
filterId = (0); //
|
|
mask = (0); //
|
|
sFilterConfig.FilterBank = m_config->canFilterIndex0; //
|
|
sFilterConfig.FilterMaskIdLow = mask & 0xffff; //
|
|
sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16; //
|
|
sFilterConfig.FilterIdLow = filterId & 0xffff; //
|
|
sFilterConfig.FilterIdHigh = (filterId & 0xffff0000) >> 16; //
|
|
HAL_Status = HAL_CAN_ConfigFilter(m_config->canHandle, &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;
|
|
}
|
|
|
|
int32_t ZCanCommnaderMaster::sendCmd(int32_t cmdid, int32_t submoduleid, int32_t *param, size_t npara, int32_t *ack, size_t nack, int overtime_ms) {
|
|
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)txbuff;
|
|
cmdheader->packetType = kptv2_cmd;
|
|
cmdheader->packetindex = generateFreeIndex();
|
|
cmdheader->cmdmoduleid = MODULE_CMDID(cmdid);
|
|
cmdheader->subcmdid = SUBCMDID(cmdid);
|
|
cmdheader->submoduleid = submoduleid;
|
|
int32_t *sendparam = (int32_t *)cmdheader->data;
|
|
for (size_t i = 0; i < npara; i++) {
|
|
sendparam[i] = param[i];
|
|
}
|
|
|
|
int32_t txlen = sizeof(zcr_cmd_header_t) + npara * sizeof(int32_t);
|
|
|
|
/**
|
|
* @brief 注册监听者
|
|
*/
|
|
bool rxdataIsReady = false;
|
|
int32_t errocode = 0;
|
|
regListener(cmdheader->packetindex, [this, &rxdataIsReady, &ack, &nack, &errocode](CanPacketRxBuffer *report) {
|
|
if (report->get_cmdheader()->packetType == kptv2_error_ack) {
|
|
auto *error_ack = report->get_data_as<int32_t>();
|
|
errocode = *error_ack;
|
|
} else {
|
|
int32_t *rxbuf = report->get_data_as<int32_t>();
|
|
if (ack != nullptr && nack != 0) {
|
|
for (size_t i = 0; i < nack; i++) {
|
|
ack[i] = rxbuf[i];
|
|
}
|
|
}
|
|
}
|
|
rxdataIsReady = true;
|
|
});
|
|
|
|
/**
|
|
* @brief 发送消息
|
|
*/
|
|
sendPacket(txbuff, txlen);
|
|
|
|
/**
|
|
* @brief 等待回执
|
|
*/
|
|
uint32_t enterticket = zos_get_tick();
|
|
while (!rxdataIsReady) {
|
|
if (zos_haspassedms(enterticket) > (uint32_t)overtime_ms) {
|
|
ZLOGE(TAG, "sendPacketBlock timeout");
|
|
unregListener(cmdheader->packetindex);
|
|
return err::kovertime;
|
|
}
|
|
osDelay(1);
|
|
}
|
|
unregListener(cmdheader->packetindex);
|
|
|
|
return errocode;
|
|
}
|
|
|
|
void ZCanCommnaderMaster::regListener(uint16_t index, zcan_commnader_master_onpacket_t onack) {
|
|
zlock_guard l(m_on_packet_map_lock);
|
|
if (m_on_packet_map.size() > 10000) {
|
|
ZLOGW(TAG, "m_on_packet_map.size() = %d>10000", m_on_packet_map.size());
|
|
}
|
|
ZCanCommnaderMasterListener listener;
|
|
listener.on_ack = onack;
|
|
m_on_packet_map[index] = listener;
|
|
}
|
|
void ZCanCommnaderMaster::unregListener(uint16_t index) {
|
|
zlock_guard l(m_on_packet_map_lock);
|
|
auto it = m_on_packet_map.find(index);
|
|
if (it != m_on_packet_map.end()) {
|
|
m_on_packet_map.erase(it);
|
|
}
|
|
}
|
|
|
|
int ZCanCommnaderMaster::getListenerNum() {
|
|
zlock_guard l(m_on_packet_map_lock);
|
|
return m_on_packet_map.size();
|
|
}
|
|
|
|
bool ZCanCommnaderMaster::isListenerReg(uint16_t index) {
|
|
zlock_guard l(m_on_packet_map_lock);
|
|
auto it = m_on_packet_map.find(index);
|
|
if (it != m_on_packet_map.end()) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void ZCanCommnaderMaster::callListener(CanPacketRxBuffer *report) {
|
|
uint16_t index = report->get_cmdheader()->packetindex;
|
|
|
|
{
|
|
zlock_guard l(m_on_packet_map_lock);
|
|
auto it = m_on_packet_map.find(index);
|
|
if (it != m_on_packet_map.end()) {
|
|
if (report->get_cmdheader()->packetType == kptv2_ack || report->get_cmdheader()->packetType == kptv2_error_ack) {
|
|
if (it->second.on_ack) it->second.on_ack(report);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
uint16_t ZCanCommnaderMaster::generateFreeIndex() {
|
|
m_index_off++;
|
|
uint16_t count = 0;
|
|
if (m_index_off == 0) m_index_off = 1;
|
|
while (isListenerReg(m_index_off)) {
|
|
m_index_off++;
|
|
if (m_index_off == 0) m_index_off = 1;
|
|
count++;
|
|
if (count == 0) {
|
|
ZLOGE(TAG, "generateFreeIndex fail");
|
|
NVIC_SystemReset();
|
|
}
|
|
}
|
|
return m_index_off;
|
|
}
|
|
|
|
void ZCanCommnaderMaster::sendPacket(uint8_t *packet, size_t len) {
|
|
zlock_guard txlock_guard(txlock);
|
|
/**
|
|
* @brief
|
|
*/
|
|
int npacket = len / 8 + (len % 8 == 0 ? 0 : 1);
|
|
if (npacket > 255) {
|
|
ZLOGE(TAG, "sendPacket fail, len:%d", len);
|
|
return;
|
|
}
|
|
int finalpacketlen = len % 8 == 0 ? 8 : len % 8;
|
|
|
|
for (uint8_t i = 0; i < npacket; i++) {
|
|
bool suc = false;
|
|
if (i == npacket - 1) {
|
|
suc = sendPacketSub(npacket, i, packet + i * 8, finalpacketlen, OVER_TIME_MS);
|
|
} else {
|
|
suc = sendPacketSub(npacket, i, packet + i * 8, 8, OVER_TIME_MS);
|
|
}
|
|
if (!suc) {
|
|
ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool ZCanCommnaderMaster::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) {
|
|
// ZLOGI(TAG, "sendPacketSub(%d:%d)", npacket, packetIndex);
|
|
CAN_TxHeaderTypeDef pHeader;
|
|
uint8_t aData[8] /*8byte table*/;
|
|
uint32_t txMailBox = 0;
|
|
|
|
uint32_t enterticket = zos_get_tick();
|
|
|
|
memset(&pHeader, 0, sizeof(pHeader));
|
|
memset(aData, 0, sizeof(aData));
|
|
pHeader.StdId = 0x00;
|
|
pHeader.ExtId = (m_config->deviceId << 16) | (npacket << 8) | packetIndex;
|
|
pHeader.IDE = CAN_ID_EXT;
|
|
pHeader.RTR = CAN_RTR_DATA;
|
|
pHeader.DLC = len;
|
|
pHeader.TransmitGlobalTime = DISABLE;
|
|
|
|
memcpy(aData, packet, len);
|
|
|
|
m_lastTransmitStatus = HAL_CAN_AddTxMessage(m_config->canHandle, &pHeader, aData, &txMailBox);
|
|
if (m_lastTransmitStatus != HAL_OK) {
|
|
ZLOGE(TAG, "HAL_CAN_AddTxMessage fail");
|
|
return false;
|
|
}
|
|
|
|
while (HAL_CAN_IsTxMessagePending(m_config->canHandle, txMailBox)) {
|
|
if (zos_haspassedms(enterticket) > (uint32_t)overtimems) {
|
|
m_lastTransmitStatus = HAL_TIMEOUT;
|
|
HAL_CAN_AbortTxRequest(m_config->canHandle, txMailBox);
|
|
return false;
|
|
}
|
|
// m_os->sleepMS(1);
|
|
}
|
|
if (txPacketInterval_ms > 0) {
|
|
osDelay(txPacketInterval_ms);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ZCanCommnaderMaster::getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/) {
|
|
/**
|
|
* @brief 读取当前FIFO中缓存了多少帧的数据
|
|
*/
|
|
uint32_t level = HAL_CAN_GetRxFifoFillLevel(m_config->canHandle, m_config->rxfifoNum);
|
|
if (level == 0) {
|
|
return false;
|
|
}
|
|
HAL_StatusTypeDef HAL_RetVal;
|
|
HAL_RetVal = HAL_CAN_GetRxMessage(m_config->canHandle, m_config->rxfifoNum, pHeader, aData);
|
|
if (HAL_OK == HAL_RetVal) {
|
|
// 处理接收到的can总线数据
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
void ZCanCommnaderMaster::initCanPacketRxBuffer(CanPacketRxBuffer *buf, uint16_t id) {
|
|
memset(buf, 0, sizeof(CanPacketRxBuffer));
|
|
buf->id = id;
|
|
}
|
|
|
|
CanPacketRxBuffer *ZCanCommnaderMaster::allocCanPacketRxBufferInIRQ(uint16_t id) {
|
|
for (size_t i = 0; i < CAN_PACKET_RX_BUFFER_NUM; i++) {
|
|
if (m_canPacketRxBuffer[i].isUsed && m_canPacketRxBuffer[i].id == id) {
|
|
// 说明当前id的缓存已经存在，但由于丢包数据灭有收集满
|
|
if (!m_canPacketRxBuffer[i].dataIsReady) {
|
|
initCanPacketRxBuffer(&m_canPacketRxBuffer[i], id);
|
|
m_canPacketRxBuffer[i].isUsed = true;
|
|
return &m_canPacketRxBuffer[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
for (size_t i = 0; i < CAN_PACKET_RX_BUFFER_NUM; i++) {
|
|
if (!m_canPacketRxBuffer[i].isUsed) {
|
|
initCanPacketRxBuffer(&m_canPacketRxBuffer[i], id);
|
|
m_canPacketRxBuffer[i].isUsed = true;
|
|
return &m_canPacketRxBuffer[i];
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
CanPacketRxBuffer *ZCanCommnaderMaster::findCanPacketRxBufferInIRQ(uint16_t id) {
|
|
for (size_t i = 0; i < CAN_PACKET_RX_BUFFER_NUM; i++) {
|
|
if (!m_canPacketRxBuffer[i].dataIsReady && m_canPacketRxBuffer[i].isUsed && m_canPacketRxBuffer[i].id == id) {
|
|
return &m_canPacketRxBuffer[i];
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
void ZCanCommnaderMaster::freeCanPacketRxBuffer(uint16_t id) {
|
|
for (size_t i = 0; i < CAN_PACKET_RX_BUFFER_NUM; i++) {
|
|
if (m_canPacketRxBuffer[i].isUsed && m_canPacketRxBuffer[i].id == id) {
|
|
m_canPacketRxBuffer[i].isUsed = false;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ZCanCommnaderMaster::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle) {
|
|
/**
|
|
* @brief 中断上下文
|
|
*/
|
|
// ZLOG_INFO("%s\n", __FUNCTION__);
|
|
// printf("------------------%s\n", __FUNCTION__);
|
|
if (canHandle != m_config->canHandle) {
|
|
return;
|
|
}
|
|
/**
|
|
* @brief 处理can接收到消息
|
|
*/
|
|
CAN_RxHeaderTypeDef pHeader;
|
|
uint8_t aData[8] /*8byte table*/;
|
|
while (getRxMessage(&pHeader, aData)) {
|
|
/**
|
|
* @brief 消息格式
|
|
*
|
|
* [2] [3bit] [8bit] [8bit] [8bit]
|
|
* , from frameNum frameId
|
|
*/
|
|
uint8_t from = (pHeader.ExtId >> 16 & 0xFF);
|
|
uint8_t nframe = (pHeader.ExtId & 0xFF00) >> 8;
|
|
uint8_t frameId = (pHeader.ExtId & 0x00FF);
|
|
CanPacketRxBuffer *rxbuf = nullptr;
|
|
if (frameId == 0) {
|
|
rxbuf = allocCanPacketRxBufferInIRQ(from);
|
|
rxbuf->m_npacket = nframe;
|
|
} else {
|
|
rxbuf = findCanPacketRxBufferInIRQ(from);
|
|
}
|
|
if (!rxbuf) return;
|
|
|
|
if (rxbuf->m_canPacketNum < ZARRAY_SIZE(rxbuf->m_canPacket)) {
|
|
rxbuf->m_canPacket[rxbuf->m_canPacketNum].pHeader = pHeader;
|
|
memcpy(rxbuf->m_canPacket[rxbuf->m_canPacketNum].aData, aData, 8);
|
|
rxbuf->m_canPacketNum++;
|
|
}
|
|
/**
|
|
* @brief 接收满了
|
|
*/
|
|
if (nframe == frameId + 1) {
|
|
rxbuf->dataIsReady = true;
|
|
if ((rxbuf->m_canPacketNum) != rxbuf->m_npacket) rxbuf->lostpacket = true;
|
|
}
|
|
}
|
|
|
|
// deactivateRxIT();
|
|
}
|
|
void ZCanCommnaderMaster::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) {
|
|
if (canHandle != m_config->canHandle) {
|
|
return;
|
|
}
|
|
ZLOGE(TAG, "onCAN_Error\r\n");
|
|
}
|
|
void ZCanCommnaderMaster::processReadyPacket(CanPacketRxBuffer *rxbuf) {
|
|
int dataoff = 0;
|
|
for (size_t i = 0; i < rxbuf->m_canPacketNum; i++) {
|
|
memcpy(rxbuf->rxdata + dataoff, rxbuf->m_canPacket[i].aData, rxbuf->m_canPacket[i].pHeader.DLC);
|
|
dataoff += rxbuf->m_canPacket[i].pHeader.DLC;
|
|
rxbuf->rxdataSize = dataoff;
|
|
}
|
|
if (rxbuf->lostpacket) {
|
|
ZLOGE(TAG, "lostpacket %d %d", rxbuf->m_canPacketNum, rxbuf->m_npacket);
|
|
} else {
|
|
callListener(rxbuf);
|
|
}
|
|
rxbuf->dataIsReady = false;
|
|
}
|
|
void ZCanCommnaderMaster::loop() {
|
|
/**
|
|
* @brief MainLoop上下文
|
|
*/
|
|
|
|
for (size_t i = 0; i < CAN_PACKET_RX_BUFFER_NUM; i++) {
|
|
if (m_canPacketRxBuffer[i].isUsed && m_canPacketRxBuffer[i].dataIsReady) {
|
|
processReadyPacket(&m_canPacketRxBuffer[i]);
|
|
}
|
|
}
|
|
}
|
|
HAL_StatusTypeDef ZCanCommnaderMaster::activateRxIT() {
|
|
HAL_StatusTypeDef hal_status = HAL_ERROR;
|
|
if (m_config->rxfifoNum == CAN_RX_FIFO0) {
|
|
hal_status = HAL_CAN_ActivateNotification(m_config->canHandle, CAN_IT_RX_FIFO0_MSG_PENDING);
|
|
} else if (m_config->rxfifoNum == CAN_RX_FIFO1) {
|
|
hal_status = HAL_CAN_ActivateNotification(m_config->canHandle, 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 ZCanCommnaderMaster::deactivateRxIT() {
|
|
HAL_StatusTypeDef hal_status = HAL_ERROR;
|
|
if (m_config->rxfifoNum == CAN_RX_FIFO0) {
|
|
hal_status = HAL_CAN_DeactivateNotification(m_config->canHandle, CAN_IT_RX_FIFO0_MSG_PENDING);
|
|
} else if (m_config->rxfifoNum == CAN_RX_FIFO1) {
|
|
hal_status = HAL_CAN_DeactivateNotification(m_config->canHandle, 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;
|
|
}
|
|
|
|
size_t ZCanCommnaderMaster::safe_memcpy(void *dst, size_t dst_max_size, void *src, size_t src_len) { //
|
|
size_t cpysize = dst_max_size < src_len ? dst_max_size : src_len;
|
|
memcpy(dst, src, cpysize);
|
|
return cpysize;
|
|
}
|
|
|
|
#endif
|