project_boditech_vidas_a8000_v3
/
a8000_subboard
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

v1021 | 修复 zcanreceiver 丢失部分帧的 bug，从机设备直接收来自canid=0的消息

master
zhaohe 4 months ago
parent
9dd2f4249b
commit
712ebfbc9d
6 changed files with 78 additions and 155 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
  2. 54
      sdk/components/zcancmder/basic.cpp
  3. 56
      sdk/components/zcancmder/basic.hpp
  4. 4
      sdk/components/zcancmder/zcan_protocol_parser.cpp
  5. 83
      sdk/components/zcancmder/zcanreceiver.cpp
  6. 21
      sdk/components/zcancmder/zcanreceiver.hpp

1
sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -103,6 +103,7 @@ int32_t MiniServoCtrlModule::mini_servo_read_reg(int32_t regoff, int32_t *regval
if (!suc) return err::ksubdevice_overtime; if (!suc) return err::ksubdevice_overtime;
return 0; return 0;
} }
return err::kparam_out_of_range;
} }
int32_t MiniServoCtrlModule::mini_servo_active_cfg() { return 0; } int32_t MiniServoCtrlModule::mini_servo_active_cfg() { return 0; }

54
sdk/components/zcancmder/basic.cpp
View File

@ -1,54 +0,0 @@
#include "basic.hpp"
#include <stdio.h>
#include <string.h>
using namespace iflytop;
using namespace std;
uint16_t CanPacketRxBuffer::get_packetindex() {
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata;
return cmdheader->index;
}
uint16_t CanPacketRxBuffer::get_cmdid() {
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata;
return cmdheader->cmdid;
}
uint8_t CanPacketRxBuffer::get_packetType() {
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata;
return cmdheader->packetType;
}
uint8_t *CanPacketRxBuffer::get_params() {
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata;
return (cmdheader->data);
}
uint16_t CanPacketRxBuffer::get_params_len() {
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata;
return cmdheader->datalen;
}
void CanPacketRxBuffer::clear() {
id = 0;
memset(&m_canPacket, 0, sizeof(m_canPacket)); // fix
m_canPacketNum = 0;
m_npacket = 0;
memset(rxdata, 0, sizeof(rxdata));
rxdataSize = 0;
dataIsReady = false;
isOnReceive = false;
isUsed = false;
lostpacket = false;
}
bool CanPacketRxBuffer::iscmd(int32_t id) {
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata;
return cmdheader->cmdid == id;
}
zcr_cmd_header_t *CanPacketRxBuffer::get_cmdheader() {
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata;
return cmdheader;
}
uint8_t *CanPacketRxBuffer::get_rx_raw() { return rxdata; }
uint16_t CanPacketRxBuffer::get_rx_raw_len() { return rxdataSize; }

56
sdk/components/zcancmder/basic.hpp
View File

@ -5,59 +5,3 @@
#pragma once #pragma once
#include "a8000_protocol\protocol.hpp" #include "a8000_protocol\protocol.hpp"
#include "sdk/os/zos.hpp" #include "sdk/os/zos.hpp"
namespace iflytop {
namespace zcr {
typedef enum {
kpacketHeader = 0,
kpacketData = 1,
kpacketTail = 2,
} packet_type_t;
typedef struct {
// CAN_RxHeaderTypeDef pHeader;
uint8_t dlc;
uint8_t aData[8]; /*8byte table*/
} CANPacket;
}; // namespace zcr
using namespace zcr;
class CanPacketRxBuffer {
public:
uint16_t id = 0;
CANPacket m_canPacket[20] = {0}; // 用于接收can消息
uint8_t m_canPacketNum = 0;
uint8_t m_npacket = 0;
uint8_t rxdata[256] = {0};
int rxdataSize = 0;
bool dataIsReady = false;
bool isOnReceive = false;
bool isUsed = false;
bool lostpacket = false;
bool dataIsProcessed = false;
int32_t lastrxtime = 0;
public:
uint16_t get_packetindex();
uint16_t get_cmdid();
uint8_t get_packetType();
uint8_t *get_params();
uint16_t get_params_len();
void clear();
uint8_t *get_rx_raw();
uint16_t get_rx_raw_len();
zcr_cmd_header_t *get_cmdheader();
bool iscmd(int32_t id);
template <typename T>
T *get_param_as() {
return (T *)get_params();
}
};
} // namespace iflytop

4
sdk/components/zcancmder/zcan_protocol_parser.cpp
View File

@ -186,6 +186,8 @@ void ZCanProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, int32_t len) {
return; return;
} }
// ZLOGI(TAG, "rx(index:%d) cmdid:%d moduleid:%d", rxcmd->index,rxcmd->cmdid, moduleid);
ZIModule* module = it->second; ZIModule* module = it->second;
int32_t cmdid = rxcmd->cmdid; int32_t cmdid = rxcmd->cmdid;
_onRceivePacket(module, rxcmd, len); _onRceivePacket(module, rxcmd, len);
@ -733,8 +735,6 @@ int32_t ZCanProtocolParser::a8k_opt_v2_f_readVal(cmdcontxt_t* cxt) {
return module->a8k_opt_v2_f_readVal(&ack[0], &ack[1]); return module->a8k_opt_v2_f_readVal(&ack[0], &ack[1]);
} }
#undef MODULE_CLASS #undef MODULE_CLASS
#define MODULE_CLASS EEPROMService #define MODULE_CLASS EEPROMService

83
sdk/components/zcancmder/zcanreceiver.cpp
View File

@ -45,9 +45,9 @@ void ZCanReceiver::initialize(CFG *cfg) {
/** /**
* @brief 初始化消息接收buf * @brief 初始化消息接收buf
*/ */
m_canPacketRxBuffer[0].dataIsReady = false;
m_canPacketRxBuffer[0].id = 0; // 只接收来自主机的消息
m_canPacketRxBuffer[0].m_canPacketNum = 0;
m_rxFrameBuffer.dataIsReady = false;
m_rxFrameBuffer.id = 0; // 只接收来自主机的消息
m_rxFrameBuffer.canPacketNum = 0;
/** /**
* @brief 初始化过滤器 * @brief 初始化过滤器
@ -94,10 +94,11 @@ HAL_StatusTypeDef ZCanReceiver::initializeFilter() {
sFilterConfig.SlaveStartFilterBank = m_config->maxFilterNum; // slave filter start index sFilterConfig.SlaveStartFilterBank = m_config->maxFilterNum; // slave filter start index
/******************************************************************************* /*******************************************************************************
* 接收所有消息 *
* 直接收来自主机的消息 *
*******************************************************************************/ *******************************************************************************/
filterId = (0); // filterId = (0); //
mask = (0); // mask = (0); //
mask = (0xffffffff); //
sFilterConfig.FilterBank = m_config->canFilterIndex0; // sFilterConfig.FilterBank = m_config->canFilterIndex0; //
sFilterConfig.FilterMaskIdLow = mask & 0xffff; // sFilterConfig.FilterMaskIdLow = mask & 0xffff; //
sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16; // sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16; //
@ -123,6 +124,7 @@ void ZCanReceiver::registerListener(IZCanReceiverListener *listener) { m_listene
// return buf; // return buf;
// } // }
static const char *hex2str(const char *hex, size_t len) __attribute__((unused));
static const char *hex2str(const char *hex, size_t len) { static const char *hex2str(const char *hex, size_t len) {
static char buf[256]; static char buf[256];
memset(buf, 0, sizeof(buf)); memset(buf, 0, sizeof(buf));
@ -291,7 +293,6 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl
/** /**
* @brief 中断上下文 * @brief 中断上下文
*/ */
// ZLOG_INFO("%s\n", __FUNCTION__);
if (canHandle != m_config->canHandle) { if (canHandle != m_config->canHandle) {
return; return;
} }
@ -300,14 +301,14 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl
*/ */
CAN_RxHeaderTypeDef pHeader; CAN_RxHeaderTypeDef pHeader;
uint8_t aData[8] = {0}; uint8_t aData[8] = {0};
CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0];
// CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer;
while (getRxMessage(&pHeader, aData)) { while (getRxMessage(&pHeader, aData)) {
// 过滤掉非标准帧和远程帧，和DLC为0的帧 // 过滤掉非标准帧和远程帧，和DLC为0的帧
if (pHeader.RTR != CAN_RTR_DATA || pHeader.IDE != CAN_ID_STD) continue; if (pHeader.RTR != CAN_RTR_DATA || pHeader.IDE != CAN_ID_STD) continue;
if (pHeader.DLC == 0) continue; if (pHeader.DLC == 0) continue;
// 上次接收到的消息还没有来的急处理 // 上次接收到的消息还没有来的急处理
if (rxbuf->dataIsReady) continue;
if (m_rxFrameBuffer.dataIsReady) continue;
/** /**
* @brief 处理接收到的数据 * @brief 处理接收到的数据
@ -318,29 +319,29 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl
uint8_t frameoff = (aData[0] & 0x0F); uint8_t frameoff = (aData[0] & 0x0F);
// 只接收来自主机的消息 // 只接收来自主机的消息
if (from != rxbuf->id) continue;
if (from != m_rxFrameBuffer.id) continue;
// 当frameoff==0,重置接收缓存 // 当frameoff==0,重置接收缓存
if (frameoff == 0) { if (frameoff == 0) {
rxbuf->m_canPacketNum = 0;
m_rxFrameBuffer.canPacketNum = 0;
} }
// 当接收到非期望frameoff的数据时，说明发生了丢包，重置接收缓存 // 当接收到非期望frameoff的数据时，说明发生了丢包，重置接收缓存
if (rxbuf->m_canPacketNum != frameoff) {
rxbuf->m_canPacketNum = 0;
if (m_rxFrameBuffer.canPacketNum != frameoff) {
m_rxFrameBuffer.canPacketNum = 0;
continue; 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 (m_rxFrameBuffer.canPacketNum < ZARRAY_SIZE(m_rxFrameBuffer.canPacket)) {
m_rxFrameBuffer.canPacket[m_rxFrameBuffer.canPacketNum].dlc = pHeader.DLC;
memcpy(m_rxFrameBuffer.canPacket[m_rxFrameBuffer.canPacketNum].aData, aData, 8);
m_rxFrameBuffer.canPacketNum++;
} }
if (nframe == frameoff + 1) { if (nframe == frameoff + 1) {
rxbuf->m_npacket = nframe;
rxbuf->dataIsReady = true;
m_rxFrameBuffer.npacket = nframe;
m_rxFrameBuffer.dataIsReady = true;
} }
} }
@ -352,29 +353,45 @@ void ZCanReceiver::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) {
} }
ZLOGE(TAG, "onCAN_Error\r\n"); ZLOGE(TAG, "onCAN_Error\r\n");
} }
void ZCanReceiver::loop() {
CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0];
if (rxbuf->dataIsReady) {
if (rxbuf->m_canPacketNum != rxbuf->m_npacket) {
ZLOGE(TAG, "lost packet");
} else {
int dataoff = 0;
for (size_t i = 0; i < rxbuf->m_canPacketNum; i++) {
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_rx_raw_len());
void ZCanReceiver::processRx() {
auto *rxbuf = &m_rxFrameBuffer;
if (!rxbuf->dataIsReady) return;
if (rxbuf->canPacketNum != rxbuf->npacket) {
ZLOGE(TAG, "lost packet");
rxbuf->canPacketNum = 0;
rxbuf->npacket = 0;
rxbuf->dataIsReady = false;
return;
} }
//
// !处理接收到的数据!
//
// 拷贝接收到的数据到processbuf中
uint8_t *processbuf = m_rxPacketProcessBuf;
int32_t processlen = 0;
for (size_t i = 0; i < rxbuf->canPacketNum; i++) {
if (rxbuf->canPacket[i].dlc == 0) continue;
memcpy(processbuf + processlen, rxbuf->canPacket[i].aData + 1, rxbuf->canPacket[i].dlc - 1);
processlen += rxbuf->canPacket[i].dlc - 1;
} }
// 重新开始接收CAN总线上的数据
rxbuf->dataIsReady = false; rxbuf->dataIsReady = false;
rxbuf->canPacketNum = 0;
rxbuf->npacket = 0;
// 处理接收到的数据
for (auto &var : m_listenerList) {
if (var) var->onRceivePacket((zcr_cmd_header_t *)processbuf, processlen);
} }
} }
void ZCanReceiver::loop() { processRx(); }
HAL_StatusTypeDef ZCanReceiver::activateRxIT() { HAL_StatusTypeDef ZCanReceiver::activateRxIT() {
HAL_StatusTypeDef hal_status = HAL_ERROR; HAL_StatusTypeDef hal_status = HAL_ERROR;
if (m_config->rxfifoNum == CAN_RX_FIFO0) { if (m_config->rxfifoNum == CAN_RX_FIFO0) {

21
sdk/components/zcancmder/zcanreceiver.hpp
View File

@ -12,8 +12,6 @@
namespace iflytop { namespace iflytop {
using namespace zcr; using namespace zcr;
typedef function<void(CanPacketRxBuffer *rxcmd)> zcanreceiver_listener_t;
class IZCanReceiverListener { class IZCanReceiverListener {
public: public:
virtual void onRceivePacket(zcr_cmd_header_t *rxcmd, int32_t len) = 0; virtual void onRceivePacket(zcr_cmd_header_t *rxcmd, int32_t len) = 0;
@ -34,6 +32,20 @@ class ZCanReceiver : public ZCanIRQListener {
int packetRxOvertime_ms; // int packetRxOvertime_ms; //
}; };
class RXFrameBuffer {
typedef struct {
uint8_t dlc;
uint8_t aData[8]; /*8byte table*/
} CANPacket;
public:
uint16_t id = 0;
CANPacket canPacket[16] = {0}; // 最多16(2^4)帧数据
uint8_t canPacketNum = 0;
uint8_t npacket = 0;
bool dataIsReady = false;
};
uint8_t txbuff[128]; uint8_t txbuff[128];
public: public:
@ -49,7 +61,8 @@ class ZCanReceiver : public ZCanIRQListener {
HAL_StatusTypeDef m_lastTransmitStatus; // 上次调用can发送方法的返回值 HAL_StatusTypeDef m_lastTransmitStatus; // 上次调用can发送方法的返回值
list<IZCanReceiverListener *> m_listenerList; list<IZCanReceiverListener *> m_listenerList;
CanPacketRxBuffer m_canPacketRxBuffer[1];
RXFrameBuffer m_rxFrameBuffer;
uint8_t m_rxPacketProcessBuf[128]; // 单包数据最大包长 112(7 * 2^4)
int txPacketInterval_ms = 0; int txPacketInterval_ms = 0;
zmutex m_lock; zmutex m_lock;
@ -80,6 +93,8 @@ class ZCanReceiver : public ZCanIRQListener {
void sendPacket(uint8_t *packet, size_t len); void sendPacket(uint8_t *packet, size_t len);
bool sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems); bool sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems);
void processRx();
HAL_StatusTypeDef initializeFilter(); HAL_StatusTypeDef initializeFilter();
HAL_StatusTypeDef activateRxIT(); HAL_StatusTypeDef activateRxIT();
HAL_StatusTypeDef deactivateRxIT(); HAL_StatusTypeDef deactivateRxIT();

Write Preview
Loading…
Cancel
Save