You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

297 lines
20 KiB

#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_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 cmdSubId:%d\n", rxcmd->cmdMainId, rxcmd->subModuleid, rxcmd->cmdSubId);
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->cmdMainId, rxcmd->cmdSubId);
/*******************************************************************************
* 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; }
virtual int32_t motor_easy_rotate(int32_t direction) { return err::koperation_not_support; };
virtual int32_t motor_easy_move_by(int32_t distance) { return err::koperation_not_support; };
virtual int32_t motor_easy_move_to(int32_t position) { return err::koperation_not_support; };
virtual int32_t motor_easy_move_to_zero(int32_t direction) { return err::koperation_not_support; };
virtual int32_t motor_easy_set_current_pos(int32_t pos) { return err::koperation_not_support; };
virtual int32_t motor_easy_move_to_io(int32_t ioindex, int32_t direction) { 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);
PROCESS_PACKET_10(kmotor_easy_rotate, ZIMotor, motor_easy_rotate);
PROCESS_PACKET_10(kmotor_easy_move_by, ZIMotor, motor_easy_move_by);
PROCESS_PACKET_10(kmotor_easy_move_to, ZIMotor, motor_easy_move_to);
PROCESS_PACKET_10(kmotor_easy_move_to_zero, ZIMotor, motor_easy_move_to_zero);
PROCESS_PACKET_10(kmotor_easy_set_current_pos, ZIMotor, motor_easy_set_current_pos);
PROCESS_PACKET_20(kmotor_easy_move_to_io, ZIMotor, motor_easy_move_to_io);
/*******************************************************************************
* 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; }
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);
#endif
#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);
}