|
|
|
@ -11,6 +11,14 @@ using namespace zcr; |
|
|
|
|
|
|
|
#define OVER_TIME_MS 5
|
|
|
|
|
|
|
|
static void assign_packet_checksum(uint8_t *packet, int len) { |
|
|
|
uint8_t checksum = 0; |
|
|
|
for (int i = 0; i < len - 1; i++) { |
|
|
|
checksum += packet[i]; |
|
|
|
} |
|
|
|
packet[len - 1] = checksum; |
|
|
|
} |
|
|
|
|
|
|
|
ZCanReceiver::CFG *ZCanReceiver::createCFG(uint8_t deviceId) { |
|
|
|
CFG *cfg = new CFG(); |
|
|
|
ZASSERT(cfg != NULL); |
|
|
|
@ -71,10 +79,6 @@ void ZCanReceiver::initialize(CFG *cfg) { |
|
|
|
HAL_StatusTypeDef ZCanReceiver::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; |
|
|
|
@ -118,23 +122,34 @@ void ZCanReceiver::registerListener(IZCanReceiverListener *listener) { m_listene |
|
|
|
// }
|
|
|
|
// return buf;
|
|
|
|
// }
|
|
|
|
|
|
|
|
static const char* hex2str(const char* hex, size_t len) { |
|
|
|
static char buf[256]; |
|
|
|
memset(buf, 0, sizeof(buf)); |
|
|
|
for (size_t i = 0; i < len; i++) { |
|
|
|
sprintf(buf + i * 2, "%02X", hex[i]); |
|
|
|
} |
|
|
|
return buf; |
|
|
|
} |
|
|
|
|
|
|
|
void ZCanReceiver::sendPacket(uint8_t *packet, size_t len) { |
|
|
|
ZLOGI(TAG, "sendPacket %s(%d)", hex2str((const char *)packet, len), len); |
|
|
|
/**
|
|
|
|
* @brief |
|
|
|
*/ |
|
|
|
int npacket = len / 8 + (len % 8 == 0 ? 0 : 1); |
|
|
|
int npacket = len / 7 + (len % 7 == 0 ? 0 : 1); |
|
|
|
if (npacket > 255) { |
|
|
|
ZLOGE(TAG, "sendPacket fail, len:%d", len); |
|
|
|
return; |
|
|
|
} |
|
|
|
int finalpacketlen = len % 8 == 0 ? 8 : len % 8; |
|
|
|
int finalpacketlen = len % 7 == 0 ? 7 : len % 7; |
|
|
|
|
|
|
|
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); |
|
|
|
suc = sendPacketSub(npacket, i, packet + i * 7, finalpacketlen, OVER_TIME_MS); |
|
|
|
} else { |
|
|
|
suc = sendPacketSub(npacket, i, packet + i * 8, 8, OVER_TIME_MS); |
|
|
|
suc = sendPacketSub(npacket, i, packet + i * 7, 7, OVER_TIME_MS); |
|
|
|
} |
|
|
|
if (!suc) { |
|
|
|
ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i); |
|
|
|
@ -142,93 +157,74 @@ void ZCanReceiver::sendPacket(uint8_t *packet, size_t len) { |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
#if 0
|
|
|
|
void ZCanReceiver::sendAck(zcr_cmd_header_t *cmdheader, uint8_t *data, size_t len) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
memcpy(txheader, cmdheader, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_ack; |
|
|
|
memcpy(txheader->data, data, len); |
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|
|
|
} |
|
|
|
void ZCanReceiver::sendErrorAck(zcr_cmd_header_t *cmdheader, uint16_t id, uint32_t errcode) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
|
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
memcpy(txheader, cmdheader, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_error_ack; |
|
|
|
zcanreceiver_error_ack_t *error_ack = (zcanreceiver_error_ack_t *)txheader->data; |
|
|
|
error_ack->id = id; |
|
|
|
error_ack->errorcode = errcode; |
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + sizeof(zcanreceiver_error_ack_t)); |
|
|
|
} |
|
|
|
|
|
|
|
void ZCanReceiver::sendExecStatusReport(zcr_cmd_header_t *rxcmdheader, uint8_t *data, size_t len) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
int32_t ZCanReceiver::sendBufAck(zcr_cmd_header_t *rx_cmd_header, uint8_t *data, int32_t len) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
int packetlen = sizeof(zcr_cmd_header_t) + len + 1; |
|
|
|
|
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
memcpy(txheader, rxcmdheader, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_cmd_exec_status_report; |
|
|
|
memcpy(txheader->data, data, len); |
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|
|
|
} |
|
|
|
void ZCanReceiver::sendStatusReport(zcr_cmd_header_t *rxcmdheader, uint8_t *data, size_t len) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
ZASSERT(sizeof(txbuff) > packetlen); |
|
|
|
|
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
memcpy(txheader, rxcmdheader, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_report; |
|
|
|
memcpy(txheader->data, data, len); |
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|
|
|
} |
|
|
|
#endif
|
|
|
|
int32_t ZCanReceiver::sendBufAck(zcr_cmd_header_t *rx_cmd_header, uint8_t *data, int32_t len) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
ZASSERT(sizeof(txbuff) > sizeof(zcr_cmd_header_t) + len); |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
memcpy(txheader, rx_cmd_header, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_ack; |
|
|
|
txheader->datalen = len; |
|
|
|
memcpy(txheader->data, data, len); |
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|
|
|
assign_packet_checksum(txbuff, packetlen); |
|
|
|
|
|
|
|
sendPacket(txbuff, packetlen); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t ZCanReceiver::triggerEvent(zcr_cmd_header_t *cmd_header, uint8_t *data, int32_t len) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
m_reportIndex++; |
|
|
|
ZASSERT(sizeof(txbuff) > sizeof(zcr_cmd_header_t) + len); |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
zlock_guard l(m_lock); |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
int packetlen = sizeof(zcr_cmd_header_t) + len + 1; |
|
|
|
|
|
|
|
ZASSERT(sizeof(txbuff) > packetlen); |
|
|
|
|
|
|
|
memcpy(txheader, cmd_header, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_event; |
|
|
|
txheader->packetindex = m_reportIndex; |
|
|
|
txheader->packetType = kptv2_event; |
|
|
|
txheader->index = m_reportIndex; |
|
|
|
txheader->datalen = len; |
|
|
|
memcpy(txheader->data, data, len); |
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|
|
|
assign_packet_checksum(txbuff, packetlen); |
|
|
|
|
|
|
|
sendPacket(txbuff, packetlen); |
|
|
|
|
|
|
|
m_reportIndex++; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t ZCanReceiver::sendAck(zcr_cmd_header_t *rx_cmd_header, int32_t *ackvar, int32_t nack) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
zlock_guard l(m_lock); |
|
|
|
|
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
int packetlen = sizeof(zcr_cmd_header_t) + sizeof(int32_t) * nack + 1; |
|
|
|
|
|
|
|
memcpy(txheader, rx_cmd_header, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_ack; |
|
|
|
|
|
|
|
int32_t *txackcache = (int32_t *)txheader->data; |
|
|
|
int32_t *txackcache = (int32_t *)txheader->data; |
|
|
|
txheader->datalen = nack * sizeof(int32_t); |
|
|
|
for (int i = 0; i < nack; i++) { |
|
|
|
txackcache[i] = ackvar[i]; |
|
|
|
} |
|
|
|
assign_packet_checksum(txbuff, packetlen); |
|
|
|
|
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + sizeof(int32_t) * nack); |
|
|
|
sendPacket(txbuff, packetlen); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t ZCanReceiver::sendErrorAck(zcr_cmd_header_t *rx_cmd_header, int32_t errorcode) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|
|
|
int32_t *txackcache = (int32_t *)txheader->data; |
|
|
|
int packetlen = sizeof(zcr_cmd_header_t) + sizeof(int32_t) * 1 + 1; |
|
|
|
|
|
|
|
memcpy(txheader, rx_cmd_header, sizeof(zcr_cmd_header_t)); |
|
|
|
txheader->packetType = kptv2_error_ack; |
|
|
|
txheader->datalen = sizeof(int32_t); |
|
|
|
txackcache[0] = errorcode; |
|
|
|
|
|
|
|
int32_t *txackcache = (int32_t *)txheader->data; |
|
|
|
txackcache[0] = errorcode; |
|
|
|
|
|
|
|
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + sizeof(int32_t)); |
|
|
|
assign_packet_checksum(txbuff, packetlen); |
|
|
|
sendPacket(txbuff, packetlen); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
@ -244,14 +240,15 @@ bool ZCanReceiver::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, |
|
|
|
|
|
|
|
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.StdId = m_config->deviceId; |
|
|
|
pHeader.ExtId = 0; |
|
|
|
pHeader.IDE = CAN_ID_STD; |
|
|
|
pHeader.RTR = CAN_RTR_DATA; |
|
|
|
pHeader.DLC = len; |
|
|
|
pHeader.DLC = len + 1; |
|
|
|
pHeader.TransmitGlobalTime = DISABLE; |
|
|
|
|
|
|
|
memcpy(aData, packet, len); |
|
|
|
aData[0] = (npacket << 4) | packetIndex; |
|
|
|
memcpy(aData + 1, packet, len); |
|
|
|
|
|
|
|
m_lastTransmitStatus = HAL_CAN_AddTxMessage(m_config->canHandle, &pHeader, aData, &txMailBox); |
|
|
|
if (m_lastTransmitStatus != HAL_OK) { |
|
|
|
@ -266,6 +263,7 @@ bool ZCanReceiver::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, |
|
|
|
return false; |
|
|
|
} |
|
|
|
// m_os->sleepMS(1);
|
|
|
|
osDelay(1); |
|
|
|
} |
|
|
|
if (txPacketInterval_ms > 0) { |
|
|
|
osDelay(txPacketInterval_ms); |
|
|
|
@ -294,7 +292,6 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl |
|
|
|
* @brief 中断上下文 |
|
|
|
*/ |
|
|
|
// ZLOG_INFO("%s\n", __FUNCTION__);
|
|
|
|
// printf("------------------%s\n", __FUNCTION__);
|
|
|
|
if (canHandle != m_config->canHandle) { |
|
|
|
return; |
|
|
|
} |
|
|
|
@ -302,45 +299,46 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl |
|
|
|
* @brief 处理can接收到消息 |
|
|
|
*/ |
|
|
|
CAN_RxHeaderTypeDef pHeader; |
|
|
|
uint8_t aData[8] /*8byte table*/; |
|
|
|
uint8_t aData[8] = {0}; |
|
|
|
CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0]; |
|
|
|
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 = &m_canPacketRxBuffer[0]; |
|
|
|
if (from != rxbuf->id) { |
|
|
|
// 目前只接收来自主机的消息
|
|
|
|
continue; |
|
|
|
} |
|
|
|
// 过滤掉非标准帧和远程帧，和DLC为0的帧
|
|
|
|
if (pHeader.RTR != CAN_RTR_DATA || pHeader.IDE != CAN_ID_STD) continue; |
|
|
|
if (pHeader.DLC == 0) continue; |
|
|
|
|
|
|
|
// 上次接收到的消息还没有来的急处理
|
|
|
|
if (rxbuf->dataIsReady) continue; |
|
|
|
|
|
|
|
if (rxbuf->dataIsReady) { |
|
|
|
// 上次接收到的消息还没有来的急处理
|
|
|
|
continue; |
|
|
|
} |
|
|
|
/**
|
|
|
|
* @TODO:判断是否丢包 |
|
|
|
* @brief 处理接收到的数据 |
|
|
|
*/ |
|
|
|
if (frameId == 0) { |
|
|
|
|
|
|
|
uint8_t from = (pHeader.StdId & 0xFF); |
|
|
|
uint8_t nframe = (aData[0] & 0xF0) >> 4; |
|
|
|
uint8_t frameoff = (aData[0] & 0x0F); |
|
|
|
|
|
|
|
// 只接收来自主机的消息
|
|
|
|
if (from != rxbuf->id) continue; |
|
|
|
|
|
|
|
// 当frameoff==0,重置接收缓存
|
|
|
|
if (frameoff == 0) { |
|
|
|
rxbuf->m_canPacketNum = 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (rxbuf->m_canPacketNum != frameId) { |
|
|
|
// 当接收到非期望frameoff的数据时，说明发生了丢包，重置接收缓存
|
|
|
|
if (rxbuf->m_canPacketNum != frameoff) { |
|
|
|
rxbuf->m_canPacketNum = 0; |
|
|
|
continue; |
|
|
|
} |
|
|
|
|
|
|
|
// 接收到有效包
|
|
|
|
if (rxbuf->m_canPacketNum < ZARRAY_SIZE(rxbuf->m_canPacket)) { |
|
|
|
rxbuf->m_canPacket[rxbuf->m_canPacketNum].dlc = pHeader.DLC; |
|
|
|
memcpy(rxbuf->m_canPacket[rxbuf->m_canPacketNum].aData, aData, 8); |
|
|
|
rxbuf->m_canPacketNum++; |
|
|
|
} |
|
|
|
if (nframe == frameId + 1) { |
|
|
|
|
|
|
|
if (nframe == frameoff + 1) { |
|
|
|
rxbuf->m_npacket = nframe; |
|
|
|
rxbuf->dataIsReady = true; |
|
|
|
} |
|
|
|
@ -362,13 +360,15 @@ void ZCanReceiver::loop() { |
|
|
|
} else { |
|
|
|
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].dlc); |
|
|
|
dataoff += rxbuf->m_canPacket[i].dlc; |
|
|
|
if (rxbuf->m_canPacket[i].dlc == 0) continue; |
|
|
|
|
|
|
|
memcpy(rxbuf->rxdata + dataoff, rxbuf->m_canPacket[i].aData + 1, rxbuf->m_canPacket[i].dlc - 1); |
|
|
|
dataoff += rxbuf->m_canPacket[i].dlc - 1; |
|
|
|
} |
|
|
|
rxbuf->rxdataSize = dataoff; |
|
|
|
// ZLOGI(TAG,"rx packet");
|
|
|
|
for (auto &var : m_listenerList) { |
|
|
|
if (var) var->onRceivePacket(rxbuf->get_cmdheader(), rxbuf->get_params(), rxbuf->get_params_len()); |
|
|
|
if (var) var->onRceivePacket(rxbuf->get_cmdheader(), rxbuf->get_rx_raw_len()); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
