p_graphite_digester_sdk/components/zcancmder/zcanreceiver_master.cpp

#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();

  /**
   * @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::sendCmdWithStatusCb(uint8_t *packet, size_t len, uint16_t &packetindex, int overtime_ms, action_cb_status_t status_cb) {
  return sendPacketBlock(packet, len, packetindex, NULL, 0, overtime_ms, [status_cb](CanPacketRxBuffer *rxreport) {
    if (status_cb) status_cb(rxreport->get_data_as<kcmd_xy_robot_ctrl_move_to_report_t>()->exec_status);
  });
}

int32_t ZCanCommnaderMaster::sendCmd(uint8_t *packet, size_t len, uint8_t *rxbuf, size_t rxbuflen, int overtime_ms) {
  uint16_t packetindex = 0;
  return sendPacketBlock(packet, len, packetindex, rxbuf, rxbuflen, overtime_ms, NULL);
}
int32_t ZCanCommnaderMaster::sendCmdNoAck(uint8_t *packet, size_t len, int overtime_ms) {
  uint16_t packetindex = 0;
  return sendPacketBlock(packet, len, packetindex, NULL, 0, overtime_ms, NULL);
}

int32_t ZCanCommnaderMaster::sendPacketBlock(uint8_t                         *packet,
                                             size_t                           len,          //
                                             uint16_t                        &packetindex,  //
                                             uint8_t                         *rxbuf,        //
                                             size_t                           rxbuflen,     //
                                             int                              overtime_ms,  //
                                             zcan_commnader_master_onpacket_t onReport) {
  Cmdheader_t *cmdheader = (Cmdheader_t *)packet;
  if (packetindex != 0) {
    unregListener(packetindex);
    packetindex = 0;
  }

  cmdheader->packetindex = generateFreeIndex();

  /**
   * @brief ע��������
   */
  bool    rxdataIsReady = false;
  int32_t errocode      = 0;
  regListener(
      cmdheader->packetindex,
      [this, &rxdataIsReady, &rxbuf, &rxbuflen, &errocode](CanPacketRxBuffer *report) {
        if (report->get_cmdheader()->packetType == kpt_error_ack) {
          auto *error_ack = report->get_data_as<zcan_cmder_error_ack_t>();
          errocode        = error_ack->errorcode;
        } else {
          if (rxbuf) {
            // ZLOGI(TAG, "rxbuf:%d", report->get_datalen());
            safe_memcpy(rxbuf, rxbuflen, report->get_data(), report->get_datalen());
          }
        }
        rxdataIsReady = true;
      },
      onReport, nullptr);

  /**
   * @brief ������Ϣ
   */
  sendPacket(packet, len);

  /**
   * @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::kce_overtime;
    }
    osDelay(1);
  }

  if (errocode != 0) {
    unregListener(cmdheader->packetindex);
    return errocode;
  }

  packetindex = cmdheader->packetindex;
  if (!onReport) {
    unregListener(cmdheader->packetindex);
  } else {
    unregListenerAckCB(cmdheader->packetindex);
  }
  return 0;
}

void ZCanCommnaderMaster::regListener(uint16_t                         index,                      //
                                      zcan_commnader_master_onpacket_t onack,                      //
                                      zcan_commnader_master_onpacket_t on_cmd_exec_status_report,  //
                                      zcan_commnader_master_onpacket_t on_report) {
  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;
  listener.on_cmd_exec_status_report = on_cmd_exec_status_report;
  listener.on_report                 = on_report;
  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);
  }
}
void ZCanCommnaderMaster::unregListenerAckCB(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()) {
    it->second.on_ack = nullptr;
  }
}

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 == kpt_ack || report->get_cmdheader()->packetType == kpt_error_ack) {
        if (it->second.on_ack) it->second.on_ack(report);
      } else if (report->get_cmdheader()->packetType == kpt_cmd_exec_status_report) {
        if (it->second.on_cmd_exec_status_report) it->second.on_cmd_exec_status_report(report);
      } else if (report->get_cmdheader()->packetType == kpt_report) {
        if (it->second.on_report) it->second.on_report(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) {
  /**
   * @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