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.
126 lines
8.7 KiB
126 lines
8.7 KiB
#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::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) {
|
|
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);
|
|
}
|