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 __attribute__((__unused__))    = (int32_t*)data;                                     \
  int      paramNum __attribute__((__unused__)) = (len - sizeof(zcr_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::koperation_not_support);                                   \
      return;                                                                                         \
    }

#define CHECK_PARAM_NUM(num)                                    \
  if (paramNum != num) {                                        \
    m_cancmder->sendErrorAck(rxcmd, err::kcmd_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])

#define PROCESS_PACKET_40(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 0, param[0], param[1], param[2], param[3])
#define PROCESS_PACKET_41(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 1, param[0], param[1], param[2], param[3], &ack[1])
#define PROCESS_PACKET_42(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 2, param[0], param[1], param[2], param[3], &ack[1], &ack[2])
#define PROCESS_PACKET_43(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 3, param[0], param[1], param[2], param[3], &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(zcr_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, module_stop);
  PROCESS_PACKET_00(kmodule_break, ZIModule, module_break);
  PROCESS_PACKET_01(kmodule_get_last_exec_status, ZIModule, module_get_last_exec_status);
  PROCESS_PACKET_01(kmodule_get_status, ZIModule, module_get_status);
  PROCESS_PACKET_20(kmodule_set_param, ZIModule, module_set_param);
  PROCESS_PACKET_11(kmodule_get_param, ZIModule, module_get_param);
  PROCESS_PACKET_01(kmodule_readio, ZIModule, module_readio);
  PROCESS_PACKET_10(kmodule_writeio, ZIModule, module_writeio);
  PROCESS_PACKET_11(kmodule_read_adc, ZIModule, module_read_adc);
  PROCESS_PACKET_01(kmodule_get_error, ZIModule, module_get_error);
  PROCESS_PACKET_00(kmodule_clear_error, ZIModule, module_clear_error);
  PROCESS_PACKET_10(kmodule_set_inited_flag, ZIModule, module_set_inited_flag);
  PROCESS_PACKET_01(kmodule_get_inited_flag, ZIModule, module_get_inited_flag);
  /*******************************************************************************
   *                                   motor                                    *
   *******************************************************************************/
  PROCESS_PACKET_10(kmotor_enable, ZIMotor, motor_enable);
  PROCESS_PACKET_30(kmotor_rotate, ZIMotor, motor_rotate);
  PROCESS_PACKET_30(kmotor_move_by, ZIMotor, motor_move_by);
  PROCESS_PACKET_30(kmotor_move_to, ZIMotor, motor_move_to);
  PROCESS_PACKET_20(kmotor_move_to_with_torque, ZIMotor, motor_move_to_with_torque);
  PROCESS_PACKET_30(kmotor_rotate_acctime, ZIMotor, motor_rotate_acctime);
  PROCESS_PACKET_30(kmotor_move_by_acctime, ZIMotor, motor_move_by_acctime);
  PROCESS_PACKET_30(kmotor_move_to_acctime, ZIMotor, motor_move_to_acctime);
  PROCESS_PACKET_40(kmotor_move_to_zero_forward, ZIMotor, motor_move_to_zero_forward);
  PROCESS_PACKET_40(kmotor_move_to_zero_backward, ZIMotor, motor_move_to_zero_backward);
  PROCESS_PACKET_01(kmotor_read_pos, ZIMotor, motor_read_pos);
  PROCESS_PACKET_10(kmotor_set_current_pos_by_change_shift, ZIMotor, motor_set_current_pos_by_change_shift);

  /*******************************************************************************
   *                                   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);
}