a8000_subboard/sdk/components/zcancmder/zcanreceiver.cpp

#include "zcanreceiver.hpp"

#ifdef HAL_CAN_MODULE_ENABLED
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
using namespace iflytop;
using namespace zcr;

#define TAG "ZCanCmder"

#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);
  cfg->deviceId = deviceId;
#ifdef STM32F103xB
  cfg->canHandle = &hcan;
#else
  cfg->canHandle = &hcan1;
#endif
  cfg->canFilterIndex0 = 0;
  cfg->maxFilterNum    = 7;
  cfg->rxfifoNum       = CAN_RX_FIFO0;
  return cfg;
}
void ZCanReceiver::initialize(CFG *cfg) {
  HAL_StatusTypeDef hal_status;
  m_config = cfg;
  m_lock.init();

  /**
   * @brief 初始化CAN
   */

  /**
   * @brief 初始化消息接收buf
   */
  m_canPacketRxBuffer[0].dataIsReady    = false;
  m_canPacketRxBuffer[0].id             = 0;  // 只接收来自主机的消息
  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;
  }
  // ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context &context) { loop(); }, 0);
}
HAL_StatusTypeDef ZCanReceiver::initializeFilter() {
  /**
   * @brief 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;
}

void ZCanReceiver::registerListener(IZCanReceiverListener *listener) { m_listenerList.push_back(listener); }

// static inline const char *dumphex(uint8_t *packet, size_t len) {
//   static char buf[1024];
//   memset(buf, 0, sizeof(buf));
//   for (size_t i = 0; i < len; i++) {
//     sprintf(buf + i * 2, "%02x", packet[i]);
//   }
//   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, "tx %s(%d)", hex2str((const char *)packet, len), len);
  /**
   * @brief
   */
  int npacket = len / 7 + (len % 7 == 0 ? 0 : 1);
  if (npacket > 255) {
    ZLOGE(TAG, "sendPacket fail, len:%d", len);
    return;
  }
  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 * 7, finalpacketlen, OVER_TIME_MS);
    } else {
      suc = sendPacketSub(npacket, i, packet + i * 7, 7, OVER_TIME_MS);
    }
    if (!suc) {
      ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i);
      return;
    }
  }
}

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;

  ZASSERT(sizeof(txbuff) > packetlen);

  memcpy(txheader, rx_cmd_header, sizeof(zcr_cmd_header_t));
  txheader->packetType = kptv2_ack;
  txheader->datalen    = len;
  memcpy(txheader->data, data, 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);
  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->index      = m_reportIndex;
  txheader->datalen    = len;
  memcpy(txheader->data, data, 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;
  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;
  txheader->datalen    = nack * sizeof(int32_t);
  for (int i = 0; i < nack; i++) {
    txackcache[i] = ackvar[i];
  }
  assign_packet_checksum(txbuff, packetlen);

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

  assign_packet_checksum(txbuff, packetlen);
  sendPacket(txbuff, packetlen);
  return 0;
}

bool ZCanReceiver::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) {
  zlock_guard l(m_lock);

  // 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              = m_config->deviceId;
  pHeader.ExtId              = 0;
  pHeader.IDE                = CAN_ID_STD;
  pHeader.RTR                = CAN_RTR_DATA;
  pHeader.DLC                = len + 1;
  pHeader.TransmitGlobalTime = DISABLE;

  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) {
    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);
    osDelay(1);
  }
  if (txPacketInterval_ms > 0) {
    osDelay(txPacketInterval_ms);
  }
  return true;
}

bool ZCanReceiver::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 ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle) {
  /**
   * @brief 中断上下文
   */
  // ZLOG_INFO("%s\n", __FUNCTION__);
  if (canHandle != m_config->canHandle) {
    return;
  }
  /**
   * @brief 处理can接收到消息
   */
  CAN_RxHeaderTypeDef pHeader;
  uint8_t             aData[8] = {0};
  CanPacketRxBuffer  *rxbuf    = &m_canPacketRxBuffer[0];
  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;

    /**
     * @brief 处理接收到的数据
     */

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

    // 当接收到非期望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 == frameoff + 1) {
      rxbuf->m_npacket   = nframe;
      rxbuf->dataIsReady = true;
    }
  }

  //   deactivateRxIT();
}
void ZCanReceiver::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) {
  if (canHandle != m_config->canHandle) {
    return;
  }
  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;
  }
}
HAL_StatusTypeDef ZCanReceiver::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 ZCanReceiver::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;
}

#endif