zcancmder_v2/protocol_parser.cpp

#include "protocol_parser.hpp"

using namespace iflytop;
using namespace std;
#define TAG "ZIProtocolParser"

#define PROCESS_PACKET_BEGIN(var_cmdid, var_moduleType)                \
  int32_t* param    = (int32_t*)data;                                  \
  int      paramNum = (len - sizeof(cmd_header_t)) / sizeof(int32_t);  \
  if (cmdid == var_cmdid) {                                            \
    auto* mod = dynamic_cast<var_moduleType*>(module);                 \
    if (mod == nullptr) {                                              \
      m_cancmder->sendErrorAck(rxcmd, err::kce_operation_not_support); \
      return;                                                          \
    }

#define CHECK_PARAM_NUM(num)                                       \
  if (paramNum != num) {                                           \
    m_cancmder->sendErrorAck(rxcmd, err::kce_cmd_param_num_error); \
    return;                                                        \
  }

#define PROCESS_PACKET_XX_END(ackNum)        \
  if (ecode != 0) {                          \
    m_cancmder->sendAck(rxcmd, ack, ackNum); \
    return;                                  \
  }                                          \
  m_cancmder->sendErrorAck(rxcmd, ecode);    \
  return;                                    \
  }

#define PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, XP, XACK, ...) \
  PROCESS_PACKET_BEGIN(var_cmdid, var_moduleType)                                 \
  CHECK_PARAM_NUM(XP)                                                             \
  int32_t ack[XACK + 1] = {0};                                                    \
  int32_t ecode         = mod->var_funcname(__VA_ARGS__);                         \
  PROCESS_PACKET_XX_END(XACK)

#define PROCESS_PACKET_00(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 0, 0)
#define PROCESS_PACKET_01(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 0, 1, &ack[1])
#define PROCESS_PACKET_02(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 0, 2, &ack[1], &ack[2])
#define PROCESS_PACKET_03(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 0, 3, &ack[1], &ack[2], &ack[3])

#define PROCESS_PACKET_10(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 1, 0, param[0])
#define PROCESS_PACKET_11(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 1, 1, param[0], &ack[1])
#define PROCESS_PACKET_12(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 1, 2, param[0], &ack[1], &ack[2])
#define PROCESS_PACKET_13(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 1, 3, param[0], &ack[1], &ack[2], &ack[3])

#define PROCESS_PACKET_20(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 2, 0, param[0], param[1])
#define PROCESS_PACKET_21(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 2, 1, param[0], param[1], &ack[1])
#define PROCESS_PACKET_22(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 2, 2, param[0], param[1], &ack[1], &ack[2])
#define PROCESS_PACKET_23(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 2, 3, param[0], param[1], &ack[1], &ack[2], &ack[3])

#define PROCESS_PACKET_30(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 3, 0, param[0], param[1], param[2])
#define PROCESS_PACKET_31(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 3, 1, param[0], param[1], param[2], &ack[1])
#define PROCESS_PACKET_32(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 3, 2, param[0], param[1], param[2], &ack[1], &ack[2])
#define PROCESS_PACKET_33(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 3, 3, param[0], param[1], param[2], &ack[1], &ack[2], &ack[3])

void ZIProtocolParser::initialize(IZCanCmder* cancmder) {
  m_cancmder = cancmder;
  m_cancmder->registerListener(this);
}
void ZIProtocolParser::registerModule(uint16_t id, ZIModule* module) { m_modulers[id] = module; }
void ZIProtocolParser::onRceivePacket(cmd_header_t* rxcmd, uint8_t* data, int32_t len) {
  uint16_t submoduleid = rxcmd->submoduleid;
  auto     it          = m_modulers.find(submoduleid);
  if (it == m_modulers.end()) {
    return;
  }

  ZIModule* module = it->second;
  int32_t   cmdid  = CMDID(rxcmd->cmdmoduleid, rxcmd->subcmdid);

  /*******************************************************************************
   *                                   module                                   *
   *******************************************************************************/
  PROCESS_PACKET_00(kmodule_stop, ZIModule, stop);
  PROCESS_PACKET_00(kmodule_brake, ZIModule, brake);
  PROCESS_PACKET_01(kmodule_get_last_exec_status, ZIModule, get_last_exec_status);
  PROCESS_PACKET_01(kmodule_get_status, ZIModule, get_status);
  PROCESS_PACKET_20(kmodule_set_param, ZIModule, set_param);
  PROCESS_PACKET_11(kmodule_get_param, ZIModule, get_param);
  PROCESS_PACKET_01(kmodule_readio, ZIModule, readio);
  PROCESS_PACKET_10(kmodule_writeio, ZIModule, writeio);
  PROCESS_PACKET_11(kmodule_read_adc, ZIModule, read_adc);
  /*******************************************************************************
   *                                   motor                                    *
   *******************************************************************************/
  PROCESS_PACKET_10(kmotor_enable, ZIMotor, motor_enable);
  PROCESS_PACKET_20(kmotor_rotate, ZIMotor, motor_rotate);
  PROCESS_PACKET_20(kmotor_move_by, ZIMotor, motor_move_by);
  PROCESS_PACKET_20(kmotor_move_to, ZIMotor, motor_move_to);
  PROCESS_PACKET_20(kmotor_move_to_with_torque, ZIMotor, motor_move_to_with_torque);
  /*******************************************************************************
   *                                   xymotor                                  *
   *******************************************************************************/
  PROCESS_PACKET_10(kxymotor_enable, ZIXYMotor, xymotor_enable);
  PROCESS_PACKET_30(kxymotor_move_by, ZIXYMotor, xymotor_move_by);
  PROCESS_PACKET_30(kxymotor_move_to, ZIXYMotor, xymotor_move_to);
}