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

sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

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

sdk/components/zcancmder/basic.cpp

@@ -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; }

sdk/components/zcancmder/basic.hpp

@@ -5,59 +5,3 @@
 #pragma once
 #include "a8000_protocol\protocol.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

sdk/components/zcancmder/zcan_protocol_parser.cpp

@@ -125,7 +125,7 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) {
   REGFN(a8k_opt_v2_f_close_laster);
   REGFN(a8k_opt_v2_f_readVal);
   REGFN(a8k_opt_v2_barcode_start_scan);
-  
+
 #endif
 
   REGFN(a8000_idcard_reader_read_raw);
@@ -186,6 +186,8 @@ void ZCanProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, int32_t len) {
     return;
   }
 
+  // ZLOGI(TAG, "rx(index:%d) cmdid:%d moduleid:%d", rxcmd->index,rxcmd->cmdid, moduleid);
+
   ZIModule* module = it->second;
   int32_t   cmdid  = rxcmd->cmdid;
   _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]);
 }
 
-
-
 #undef MODULE_CLASS
 
 #define MODULE_CLASS EEPROMService

sdk/components/zcancmder/zcanreceiver.cpp

@@ -45,9 +45,9 @@ void ZCanReceiver::initialize(CFG *cfg) {
   /**
    * @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 初始化过滤器
@@ -94,10 +94,11 @@ HAL_StatusTypeDef ZCanReceiver::initializeFilter() {
   sFilterConfig.SlaveStartFilterBank = m_config->maxFilterNum;  // slave filter start index
 
   /*******************************************************************************
-   *                        接收所有消息                                          *
+   *                        直接收来自主机的消息                                   *
    *******************************************************************************/
   filterId                       = (0);                            //
-  mask                           = (0);                            //
+  mask                           = (0);                   //
+  mask                           = (0xffffffff);                   //
   sFilterConfig.FilterBank       = m_config->canFilterIndex0;      //
   sFilterConfig.FilterMaskIdLow  = mask & 0xffff;                  //
   sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16;      //
@@ -123,7 +124,8 @@ void ZCanReceiver::registerListener(IZCanReceiverListener *listener) { m_listene
 //   return buf;
 // }
 
-static const char* hex2str(const char* hex, size_t len) {
+static const char *hex2str(const char *hex, size_t len) __attribute__((unused));
+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++) {
@@ -291,7 +293,6 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl
   /**
    * @brief 中断上下文
    */
-  // ZLOG_INFO("%s\n", __FUNCTION__);
   if (canHandle != m_config->canHandle) {
     return;
   }
@@ -300,14 +301,14 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl
    */
   CAN_RxHeaderTypeDef pHeader;
   uint8_t             aData[8] = {0};
-  CanPacketRxBuffer  *rxbuf    = &m_canPacketRxBuffer[0];
+  // CanPacketRxBuffer  *rxbuf    = &m_canPacketRxBuffer;
   while (getRxMessage(&pHeader, aData)) {
     // 过滤掉非标准帧和远程帧，和DLC为0的帧
     if (pHeader.RTR != CAN_RTR_DATA || pHeader.IDE != CAN_ID_STD) continue;
     if (pHeader.DLC == 0) continue;
 
     // 上次接收到的消息还没有来的急处理
-    if (rxbuf->dataIsReady) continue;
+    if (m_rxFrameBuffer.dataIsReady) continue;
 
     /**
      * @brief 处理接收到的数据
@@ -318,29 +319,29 @@ void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandl
     uint8_t frameoff = (aData[0] & 0x0F);
 
     // 只接收来自主机的消息
-    if (from != rxbuf->id) continue;
+    if (from != m_rxFrameBuffer.id) continue;
 
     // 当frameoff==0,重置接收缓存
     if (frameoff == 0) {
-      rxbuf->m_canPacketNum = 0;
+      m_rxFrameBuffer.canPacketNum = 0;
     }
 
     // 当接收到非期望frameoff的数据时，说明发生了丢包，重置接收缓存
-    if (rxbuf->m_canPacketNum != frameoff) {
-      rxbuf->m_canPacketNum = 0;
+    if (m_rxFrameBuffer.canPacketNum != frameoff) {
+      m_rxFrameBuffer.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 (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) {
-      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");
 }
-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());
-      }
-    }
 
-    rxbuf->dataIsReady = false;
+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->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 hal_status = HAL_ERROR;
   if (m_config->rxfifoNum == CAN_RX_FIFO0) {

sdk/components/zcancmder/zcanreceiver.hpp

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