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