#include "protocol_parser.hpp" #include 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(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_BUF_REQ_XX_END() \ if (ecode == 0) { \ m_cancmder->sendBufAck(rxcmd, ackbuf, readbufsize); \ return; \ } else { \ 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 = (int32_t*)&ackbuf[0]; \ static_assert(XACK * sizeof(int32_t) < sizeof(ackbuf)); \ int32_t ecode = mod->var_funcname(__VA_ARGS__); \ PROCESS_PACKET_XX_END(XACK) \ } #define PROCESS_BUF_REQUEST_X(var_cmdid, var_moduleType, var_funcname, XP, ...) \ { \ PROCESS_PACKET_BEGIN(var_cmdid, var_moduleType) \ CHECK_PARAM_NUM(XP) \ int32_t readbufsize = ZCANCMD_READ_BUF_MAX_SIZE; \ int32_t ecode = mod->var_funcname(__VA_ARGS__); \ PROCESS_PACKET_BUF_REQ_XX_END() \ } #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[0]) #define PROCESS_PACKET_02(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 0, 2, &ack[0], &ack[1]) #define PROCESS_PACKET_03(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 0, 3, &ack[0], &ack[1], &ack[2]) #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[0]) #define PROCESS_PACKET_12(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 1, 2, param[0], &ack[0], &ack[1]) #define PROCESS_PACKET_13(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 1, 3, param[0], &ack[0], &ack[1], &ack[2]) #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[0]) #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[0], &ack[1]) #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[0], &ack[1], &ack[2]) #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[0]) #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[0], &ack[1]) #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[0], &ack[1], &ack[2]) #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[0]) #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[0], &ack[1]) #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[0], &ack[1], &ack[2]) #define PROCESS_BUF_REQUEST_1(var_cmdid, var_moduleType, var_funcname) PROCESS_BUF_REQUEST_X(var_cmdid, var_moduleType, var_funcname, 1, param[0], &ackbuf[0], &readbufsize) 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_ping() = 0; virtual int32_t module_stop() = 0; virtual int32_t module_break() = 0; virtual int32_t module_start() { return err::koperation_not_support; } 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_reg(int32_t param_id, int32_t param_value) { return err::koperation_not_support; } virtual int32_t module_get_reg(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_get_state(int32_t state_id, int32_t *state_value) { return err::koperation_not_support; } virtual int32_t module_read_raw(int32_t startadd, int32_t *data, int32_t *len) { return err::koperation_not_support; } virtual int32_t module_enable(int32_t enable) { return err::koperation_not_support; } #endif PROCESS_PACKET_00(kmodule_ping, ZIModule, module_ping); 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_reg, ZIModule, module_set_reg); PROCESS_PACKET_11(kmodule_get_reg, ZIModule, module_get_reg); PROCESS_PACKET_01(kmodule_readio, ZIModule, module_readio); PROCESS_PACKET_20(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_BUF_REQUEST_1(kmodule_read_raw, ZIModule, module_read_raw); PROCESS_PACKET_10(kmodule_enable, ZIModule, module_enable); PROCESS_PACKET_00(kmodule_start, ZIModule, module_start); /******************************************************************************* * motor * *******************************************************************************/ #if 0 virtual int32_t motor_enable(int32_t enable) { return err::koperation_not_support; } virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } virtual int32_t motor_rotate_with_torque(int32_t direction, int32_t torque) { return err::koperation_not_support; } virtual int32_t motor_move_to_torque(int32_t pos, int32_t torque, int32_t overtime) { return err::koperation_not_support; } virtual int32_t motor_move_to_zero_forward(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(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_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; } virtual int32_t motor_read_pos(int32_t* pos) { return err::koperation_not_support; } virtual int32_t motor_set_current_pos_by_change_shift(int32_t pos) { return err::koperation_not_support; } // Ò»°ãÓÃÓÚ¶æ»ú virtual int32_t motor_calculated_pos_by_move_to_zero() { 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_rotate_with_torque, ZIMotor, motor_rotate_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); PROCESS_PACKET_30(kmotor_move_to_torque, ZIMotor, motor_move_to_torque); PROCESS_PACKET_00(kmotor_calculated_pos_by_move_to_zero, ZIMotor, motor_calculated_pos_by_move_to_zero); /******************************************************************************* * xymotor * *******************************************************************************/ #if 0 virtual ~ZIXYMotor() {} 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; } virtual int32_t xymotor_calculated_pos_by_move_to_zero() { 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); PROCESS_PACKET_00(kxymotor_calculated_pos_by_move_to_zero, ZIXYMotor, xymotor_calculated_pos_by_move_to_zero); #if 0 virtual int32_t code_scaner_start_scan() { return err::koperation_not_support; } virtual int32_t code_scaner_stop_scan() { return err::koperation_not_support; } virtual int32_t code_scaner_read_scaner_result(int32_t startadd, uint8_t *data, int32_t *len) { return err::koperation_not_support; } #endif PROCESS_PACKET_00(kcode_scaner_start_scan, ZICodeScaner, code_scaner_start_scan); PROCESS_PACKET_00(kcode_scaner_stop_scan, ZICodeScaner, code_scaner_stop_scan); PROCESS_BUF_REQUEST_1(kcode_scaner_read_scaner_result, ZICodeScaner, code_scaner_read_scaner_result); #if 0 virtual int32_t pipette_ctrl_init_device() { return err::koperation_not_support; }; virtual int32_t pipette_ctrl_put_tip() { return err::koperation_not_support; }; virtual int32_t pipette_ctrl_move_to_ul(int32_t ul) { return err::koperation_not_support; }; #endif PROCESS_PACKET_00(kpipette_ctrl_init_device, ZIPipetteCtrlModule, pipette_ctrl_init_device); PROCESS_PACKET_00(kpipette_ctrl_put_tip, ZIPipetteCtrlModule, pipette_ctrl_put_tip); PROCESS_PACKET_10(kpipette_ctrl_move_to_ul, ZIPipetteCtrlModule, pipette_ctrl_move_to_ul); #if 0 virtual int32_t a8000_optical_module_power_ctrl(int32_t state) = 0; virtual int32_t a8000_optical_open_laser(int32_t type) = 0; virtual int32_t a8000_optical_close_laser(int32_t type) = 0; virtual int32_t a8000_optical_set_laster_gain(int32_t type, int32_t gain) = 0; virtual int32_t a8000_optical_set_scan_amp_gain(int32_t type, int32_t gain) = 0; virtual int32_t a8000_optical_read_scanner_adc_val(int32_t type, int32_t* adcval) = 0; virtual int32_t a8000_optical_read_laster_adc_val(int32_t type, int32_t* adcval) = 0; virtual int32_t a8000_optical_scan_current_point_amp_adc_val(int32_t type, int32_t lastergain, int32_t ampgain, int32_t* laster_fb_val, int32_t* adcval) = 0; #endif PROCESS_PACKET_10(ka8000_optical_module_power_ctrl, ZIA8000OpticalModule, a8000_optical_module_power_ctrl); PROCESS_PACKET_10(ka8000_optical_open_laser, ZIA8000OpticalModule, a8000_optical_open_laser); PROCESS_PACKET_10(ka8000_optical_close_laser, ZIA8000OpticalModule, a8000_optical_close_laser); PROCESS_PACKET_20(ka8000_optical_set_laster_gain, ZIA8000OpticalModule, a8000_optical_set_laster_gain); PROCESS_PACKET_20(ka8000_optical_set_scan_amp_gain, ZIA8000OpticalModule, a8000_optical_set_scan_amp_gain); PROCESS_PACKET_11(ka8000_optical_read_scanner_adc_val, ZIA8000OpticalModule, a8000_optical_read_scanner_adc_val); PROCESS_PACKET_11(ka8000_optical_read_laster_adc_val, ZIA8000OpticalModule, a8000_optical_read_laster_adc_val); PROCESS_PACKET_32(ka8000_optical_scan_current_point_amp_adc_val, ZIA8000OpticalModule, a8000_optical_scan_current_point_amp_adc_val); }