zcancmder_v2/protocol_parser.cpp

#include "protocol_parser.hpp"

#include <stdio.h>

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(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::registerModule(ZIModule* module) {
  int32_t moduleid = 0;
  module->getid(&moduleid);
  uint16_t id = moduleid;
  _registerModule(id, module);
}

void ZIProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, int32_t len) {
  // printf("onRceivePacket cmdid:%d submoduleid:%d subcmdid:%d\n", rxcmd->cmdmoduleid, rxcmd->submoduleid, rxcmd->subcmdid);
  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                                   *
   *******************************************************************************/

#if 0
    virtual ~ZIModule() {}

  virtual int32_t getid(int32_t *id) = 0;

  virtual int32_t module_stop()                                = 0;
  virtual int32_t module_break()                               = 0;
  virtual int32_t module_get_last_exec_status(int32_t *status) = 0;
  virtual int32_t module_get_status(int32_t *status)           = 0;
  virtual int32_t module_get_error(int32_t *iserror)           = 0;
  virtual int32_t module_clear_error()                         = 0;

  virtual int32_t module_set_param(int32_t param_id, int32_t param_value) { return err::koperation_not_support; }
  virtual int32_t module_get_param(int32_t param_id, int32_t *param_value) { return err::koperation_not_support; }

  virtual int32_t module_readio(int32_t *io) { return err::koperation_not_support; }
  virtual int32_t module_writeio(int32_t io) { return err::koperation_not_support; }

  virtual int32_t module_read_adc(int32_t adcindex, int32_t *adc) { return err::koperation_not_support; }

  virtual int32_t module_set_inited_flag(int32_t flag) {
    m_inited_flag = flag;
    return 0;
  }
  virtual int32_t module_get_inited_flag(int32_t *flag) {
    *flag = m_inited_flag;
    return 0;
  }
  // kmodule_factory_reset        = CMDID(1, 14),  // para:{}, ack:{}
  // kmodule_flush_cfg            = CMDID(1, 15),  // para:{}, ack:{}
  // kmodule_active_cfg           = CMDID(1, 16),  // para:{}, ack:{}

  virtual int32_t module_factory_reset() { return err::koperation_not_support; }
  virtual int32_t module_flush_cfg() { return err::koperation_not_support; }
  virtual int32_t module_active_cfg() { return err::koperation_not_support; }

      virtual int32_t module_set_state(int32_t state_id, int32_t state_value) { return err::koperation_not_support; }
  virtual int32_t module_get_state(int32_t state_id, int32_t *state_value) { return err::koperation_not_support; }
#endif
  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);
  PROCESS_PACKET_00(kmodule_factory_reset, ZIModule, module_factory_reset);
  PROCESS_PACKET_00(kmodule_flush_cfg, ZIModule, module_flush_cfg);
  PROCESS_PACKET_00(kmodule_active_cfg, ZIModule, module_active_cfg);
  PROCESS_PACKET_20(kmodule_set_state, ZIModule, module_set_state);
  PROCESS_PACKET_11(kmodule_get_state, ZIModule, module_get_state);



/*******************************************************************************
 *                                   motor                                    *
 *******************************************************************************/
#if 0
    virtual int32_t motor_move_to_zero_forward_and_calculated_shift(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; }
  virtual int32_t motor_move_to_zero_backward_and_calculated_shift(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; }

#endif
  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);
  PROCESS_PACKET_40(kmotor_motor_move_to_zero_forward_and_calculated_shift, ZIMotor, motor_move_to_zero_forward_and_calculated_shift);
  PROCESS_PACKET_40(kmotor_motor_move_to_zero_backward_and_calculated_shift, ZIMotor, motor_move_to_zero_backward_and_calculated_shift);

  /*******************************************************************************
   *                                   xymotor                                  *
   *******************************************************************************/
#if 0
  virtual int32_t xymotor_enable(int32_t enable) { return err::koperation_not_support; }
  virtual int32_t xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity) { return err::koperation_not_support; }
  virtual int32_t xymotor_move_to(int32_t x, int32_t y, int32_t motor_velocity) { return err::koperation_not_support; }
  virtual int32_t xymotor_move_to_zero() { return err::koperation_not_support; }
  virtual int32_t xymotor_move_to_zero_and_calculated_shift() { return err::koperation_not_support; }
  virtual int32_t xymotor_read_pos(int32_t *x, int32_t *y) { return err::koperation_not_support; }
#endif

  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);
  PROCESS_PACKET_00(kxymotor_move_to_zero, ZIXYMotor, xymotor_move_to_zero);
  PROCESS_PACKET_00(kxymotor_move_to_zero_and_calculated_shift, ZIXYMotor, xymotor_move_to_zero_and_calculated_shift);
  PROCESS_PACKET_02(kxymotor_read_pos, ZIXYMotor, xymotor_read_pos);
}