diff --git a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index ad0b064..f43ed3b 100644 --- a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/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; } diff --git a/sdk/components/zcancmder/basic.cpp b/sdk/components/zcancmder/basic.cpp index c724da7..e69de29 100644 --- a/sdk/components/zcancmder/basic.cpp +++ b/sdk/components/zcancmder/basic.cpp @@ -1,54 +0,0 @@ -#include "basic.hpp" - -#include -#include - -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; } \ No newline at end of file diff --git a/sdk/components/zcancmder/basic.hpp b/sdk/components/zcancmder/basic.hpp index b08e4e7..94b0b0f 100644 --- a/sdk/components/zcancmder/basic.hpp +++ b/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 - T *get_param_as() { - return (T *)get_params(); - } -}; - -} // namespace iflytop diff --git a/sdk/components/zcancmder/zcan_protocol_parser.cpp b/sdk/components/zcancmder/zcan_protocol_parser.cpp index b29abb5..a8cc447 100644 --- a/sdk/components/zcancmder/zcan_protocol_parser.cpp +++ b/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 diff --git a/sdk/components/zcancmder/zcanreceiver.cpp b/sdk/components/zcancmder/zcanreceiver.cpp index 9e4ae37..fadfdd8 100644 --- a/sdk/components/zcancmder/zcanreceiver.cpp +++ b/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) { diff --git a/sdk/components/zcancmder/zcanreceiver.hpp b/sdk/components/zcancmder/zcanreceiver.hpp index ee4b5fc..9b2499a 100644 --- a/sdk/components/zcancmder/zcanreceiver.hpp +++ b/sdk/components/zcancmder/zcanreceiver.hpp @@ -12,8 +12,6 @@ namespace iflytop { using namespace zcr; -typedef function 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 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();