Skip to content

Commit 1703cb9

Browse files
honghyun79honghyun79
committed
코드 최적화
 - 파싱 코드 재작성 - 중복 구조체 삭제
1 parent 78350dd commit 1703cb9

File tree

4 files changed

+160
-212
lines changed

4 files changed

+160
-212
lines changed

‎src/dxl_c/protocol.cpp

Lines changed: 52 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@
4848
//
4949
static DXLLibErrorCode_t parse_dxl1_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data);
5050
static DXLLibErrorCode_t parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data);
51-
static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data, InfoFastSyncReadInst_t *sync_read, InfoFastBulkReadInst_t *bulk_read);
51+
static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data, InfoSyncReadInst_t *sync_read, InfoBulkReadInst_t *bulk_read);
5252

5353
static DXLLibErrorCode_t add_param_to_dxl1_0_packet(InfoToMakeDXLPacket_t* p_make_packet, uint8_t *p_param, uint16_t param_len);
5454
static DXLLibErrorCode_t add_param_to_dxl2_0_packet(InfoToMakeDXLPacket_t* p_make_packet, uint8_t *p_param, uint16_t param_len);
@@ -349,19 +349,14 @@ DXLLibErrorCode_t begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet,
349349
return DXL_LIB_OK;
350350
}
351351

352-
DXLLibErrorCode_t fast_begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t protocol_ver)//, uint8_t* p_param_buf, uint16_t param_buf_capacity, uint8_t xel_count)
352+
DXLLibErrorCode_t fast_begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t protocol_ver)
353353
{
354-
// if(param_buf_capacity > 0 && p_param_buf == NULL){
355-
// return DXL_LIB_ERROR_NULLPTR;
356-
// }
357-
358354
if (protocol_ver != 2)
359355
return DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION;
360356

361357
p_parse_packet->protocol_ver = protocol_ver;
362-
// p_parse_packet->p_param_buf = p_param_buf;
363-
// p_parse_packet->param_buf_capacity = param_buf_capacity;
364-
// p_parse_packet->xel_count = xel_count;
358+
p_parse_packet->p_param_buf = nullptr;
359+
p_parse_packet->param_buf_capacity = 0;
365360
p_parse_packet->is_init = true;
366361

367362
return DXL_LIB_OK;
@@ -389,7 +384,7 @@ DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8
389384
}
390385

391386
DXLLibErrorCode_t fast_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data,
392-
InfoFastSyncReadInst_t *sync_read, InfoFastBulkReadInst_t *bulk_read)
387+
InfoSyncReadInst_t *sync_read, InfoBulkReadInst_t *bulk_read)
393388
{
394389
if (p_parse_packet == NULL)
395390
return DXL_LIB_ERROR_NULLPTR;
@@ -689,7 +684,7 @@ static DXLLibErrorCode_t parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_pac
689684

690685
//fast_parse_dxl2_0_packet
691686
static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data,
692-
InfoFastSyncReadInst_t *sync_read, InfoFastBulkReadInst_t *bulk_read)
687+
InfoSyncReadInst_t *sync_read, InfoBulkReadInst_t *bulk_read)
693688
{
694689
DXLLibErrorCode_t ret = DXL_LIB_PROCEEDING;
695690

@@ -762,25 +757,16 @@ static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_pars
762757
update_dxl_crc(&p_parse_packet->calculated_crc, recv_data);
763758
p_parse_packet->err_idx = 0;
764759
p_parse_packet->recv_param_len = 0;
765-
p_parse_packet->param_count = 0;
766-
// p_parse_packet->check_xel_count = 0;
767760

768761
if(recv_data == DXL_INST_STATUS){
769762
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_ERROR;
770763
if(p_parse_packet->packet_len < 4){ // 4 = Instruction(1)+Error(1)+CRC(2)
771764
ret = DXL_LIB_ERROR_LENGTH;
772765
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE;
773-
// }else if(p_parse_packet->packet_len > ((p_parse_packet->param_buf_capacity+4) * p_parse_packet->xel_count) + 1 ){ // 4 = Instruction(1)+Error(1)+CRC(2)
774-
// ret = DXL_LIB_ERROR_BUFFER_OVERFLOW;
775-
// p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE;
776766
}else{
777767
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_ERROR;
778768
}
779769
}else{
780-
// if(p_parse_packet->packet_len > p_parse_packet->param_buf_capacity+3){ // 3 = Instruction(1)+CRC(2)
781-
// ret = DXL_LIB_ERROR_BUFFER_OVERFLOW;
782-
// p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE;
783-
// }else
784770
if(p_parse_packet->packet_len == 3){ // 3 = Instruction(1)+CRC(2)
785771
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L;
786772
}else{
@@ -797,55 +783,62 @@ static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_pars
797783
break;
798784
}
799785
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_PARAM;
786+
787+
// p_parse_packet->fast_param_state = DXL_FAST_PARAM_ERROR;
788+
p_parse_packet->xel_index = 0;
789+
p_parse_packet->buf_index = 0;
790+
if (nullptr != sync_read)
791+
sync_read->p_xels[0].error = recv_data;
792+
else if (nullptr != bulk_read)
793+
bulk_read->p_xels[0].error = recv_data;
794+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_ID;
800795
break;
801796

802797
case DXL2_0_PACKET_PARSING_STATE_PARAM:
803798
update_dxl_crc(&p_parse_packet->calculated_crc, recv_data);
804799
// https://emanual.robotis.com/docs/en/dxl/protocol2/#parameter
805800
if (nullptr != sync_read) {
806-
int count = p_parse_packet->param_count + 1;
807-
int size = sync_read->addr_length + 4; // 4 = Error(1) + ID(1) + CRC(2)
808-
int xel_index = count / size;
809-
int index = count % size;
810-
if ((1 < index) && (index < (size - 2))) { // {Error(1), ID(1), Data(N), CRC(2)}
811-
index = index - 2;
812-
sync_read->p_xels[xel_index].p_recv_buf[index] = recv_data;
813-
}
814-
p_parse_packet->param_count += 1;
815-
p_parse_packet->recv_param_len += 1;
816-
int length = (sync_read->addr_length + 4) * sync_read->xel_count - 3; // 4 = Error(1) + ID(1) + CRC(2), 3 = Error(1) + CRC(2)
817-
if (p_parse_packet->recv_param_len == length) {
818-
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L;
801+
if (DXL_FAST_PARAM_ERROR == p_parse_packet->fast_param_state) {
802+
sync_read->p_xels[p_parse_packet->xel_index].error = recv_data;
803+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_ID;
804+
} else if (DXL_FAST_PARAM_ID == p_parse_packet->fast_param_state) {
805+
sync_read->p_xels[p_parse_packet->xel_index].id = recv_data;
806+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_DATA;
807+
} else if (DXL_FAST_PARAM_DATA == p_parse_packet->fast_param_state) {
808+
sync_read->p_xels[p_parse_packet->xel_index].p_recv_buf[p_parse_packet->buf_index] = recv_data;
809+
p_parse_packet->buf_index++;
810+
if (sync_read->addr_length == p_parse_packet->buf_index)
811+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_CRC_L;
812+
} else if (DXL_FAST_PARAM_CRC_L == p_parse_packet->fast_param_state) {
813+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_CRC_H;
814+
} else if (DXL_FAST_PARAM_CRC_H == p_parse_packet->fast_param_state) {
815+
p_parse_packet->xel_index++;
816+
p_parse_packet->buf_index = 0;
817+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_ERROR;
819818
}
820819
} else if (nullptr != bulk_read) {
821-
int count = p_parse_packet->param_count + 1;
822-
int start = 0;
823-
int end = 0;
824-
int xel_index = -1;
825-
int index = -1;
826-
for (uint8_t i = 0; i < bulk_read->xel_count; i++) {
827-
start = end;
828-
end += (bulk_read->p_xels[i].addr_length + 4); // 4 = Error(1) + ID(1) + CRC(2)
829-
if (((start + 1) < count) && (count < (end - 2))) {
830-
xel_index = i;
831-
index = count - start - 2;
832-
break;
833-
}
834-
}
835-
if (-1 != xel_index) {
836-
bulk_read->p_xels[xel_index].p_recv_buf[index] = recv_data;
837-
}
838-
p_parse_packet->param_count += 1;
839-
p_parse_packet->recv_param_len += 1;
840-
int length = 0;
841-
for (int i = 0; i < bulk_read->xel_count; i++) {
842-
length += (bulk_read->p_xels[i].addr_length + 4); // 4 = Error(1) + ID(1) + CRC(2)
843-
}
844-
length -= 3; // 3 = Error(1) + CRC(2)
845-
if (p_parse_packet->recv_param_len == length) {
846-
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L;
820+
if (DXL_FAST_PARAM_ERROR == p_parse_packet->fast_param_state) {
821+
bulk_read->p_xels[p_parse_packet->xel_index].error = recv_data;
822+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_ID;
823+
} else if (DXL_FAST_PARAM_ID == p_parse_packet->fast_param_state) {
824+
bulk_read->p_xels[p_parse_packet->xel_index].id = recv_data;
825+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_DATA;
826+
} else if (DXL_FAST_PARAM_DATA == p_parse_packet->fast_param_state) {
827+
bulk_read->p_xels[p_parse_packet->xel_index].p_recv_buf[p_parse_packet->buf_index] = recv_data;
828+
p_parse_packet->buf_index++;
829+
if (bulk_read->p_xels[p_parse_packet->xel_index].addr_length == p_parse_packet->buf_index)
830+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_CRC_L;
831+
} else if (DXL_FAST_PARAM_CRC_L == p_parse_packet->fast_param_state) {
832+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_CRC_H;
833+
} else if (DXL_FAST_PARAM_CRC_H == p_parse_packet->fast_param_state) {
834+
p_parse_packet->xel_index++;
835+
p_parse_packet->buf_index = 0;
836+
p_parse_packet->fast_param_state = DXL_FAST_PARAM_ERROR;
847837
}
848838
}
839+
p_parse_packet->recv_param_len += 1;
840+
if (p_parse_packet->recv_param_len == p_parse_packet->param_length)
841+
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L;
849842
break;
850843

851844
case DXL2_0_PACKET_PARSING_STATE_CRC_L:

‎src/dxl_c/protocol.h

Lines changed: 71 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -24,42 +24,78 @@ const int DXL_BROADCAST_ID = 0xFE;
2424

2525
namespace DYNAMIXEL {
2626

27-
typedef struct InfoSyncBulkBuffer{
27+
struct InfoFromPing_t {
28+
uint8_t id;
29+
uint16_t model_number;
30+
uint8_t firmware_version;
31+
};
32+
33+
struct InfoSyncBulkBuffer_t {
2834
uint8_t* p_buf;
2935
uint16_t buf_capacity;
3036
uint16_t gen_length;
3137
bool is_completed;
32-
} __attribute__((packed)) InfoSyncBulkBuffer_t;
38+
} __attribute__((packed));
3339

34-
typedef struct XELInfoFastSyncRead{
40+
/* Sync Instructions */
41+
struct XELInfoSyncRead_t {
3542
uint8_t *p_recv_buf;
3643
uint8_t id;
3744
uint8_t error;
38-
} __attribute__((packed)) XELInfoFastSyncRead_t;
45+
} __attribute__((packed));
46+
47+
struct InfoSyncReadInst_t {
48+
uint16_t addr;
49+
uint16_t addr_length;
50+
XELInfoSyncRead_t* p_xels;
51+
uint8_t xel_count;
52+
bool is_info_changed;
53+
InfoSyncBulkBuffer_t packet;
54+
} __attribute__((packed));
55+
56+
struct XELInfoSyncWrite_t {
57+
uint8_t* p_data;
58+
uint8_t id;
59+
} __attribute__((packed));
3960

40-
typedef struct InfoFastSyncReadInst{
61+
struct InfoSyncWriteInst_t {
4162
uint16_t addr;
4263
uint16_t addr_length;
43-
XELInfoFastSyncRead_t* p_xels;
64+
XELInfoSyncWrite_t* p_xels;
4465
uint8_t xel_count;
4566
bool is_info_changed;
4667
InfoSyncBulkBuffer_t packet;
47-
} __attribute__((packed)) InfoFastSyncReadInst_t;
68+
} __attribute__((packed));
4869

49-
typedef struct XELInfoFastBulkRead{
70+
/* Bulk Instructions */
71+
struct XELInfoBulkRead_t {
5072
uint16_t addr;
5173
uint16_t addr_length;
5274
uint8_t *p_recv_buf;
5375
uint8_t id;
5476
uint8_t error;
55-
} __attribute__((packed)) XELInfoFastBulkRead_t;
77+
} __attribute__((packed));
5678

57-
typedef struct InfoFastBulkReadInst{
58-
XELInfoFastBulkRead_t* p_xels;
79+
struct InfoBulkReadInst_t {
80+
XELInfoBulkRead_t* p_xels;
5981
uint8_t xel_count;
6082
bool is_info_changed;
6183
InfoSyncBulkBuffer_t packet;
62-
} __attribute__((packed)) InfoFastBulkReadInst_t;
84+
} __attribute__((packed));
85+
86+
struct XELInfoBulkWrite_t {
87+
uint16_t addr;
88+
uint16_t addr_length;
89+
uint8_t* p_data;
90+
uint8_t id;
91+
} __attribute__((packed));
92+
93+
struct InfoBulkWriteInst_t {
94+
XELInfoBulkWrite_t* p_xels;
95+
uint8_t xel_count;
96+
bool is_info_changed;
97+
InfoSyncBulkBuffer_t packet;
98+
} __attribute__((packed));
6399

64100
}
65101

@@ -123,16 +159,16 @@ enum DXL1_0PacketState{
123159
};
124160

125161
enum DXL2_0PacketState{
126-
DXL2_0_PACKET_PARSING_STATE_IDLE = 0,
127-
DXL2_0_PACKET_PARSING_STATE_RESERVED,
128-
DXL2_0_PACKET_PARSING_STATE_ID,
129-
DXL2_0_PACKET_PARSING_STATE_LENGTH_L,
130-
DXL2_0_PACKET_PARSING_STATE_LENGTH_H,
131-
DXL2_0_PACKET_PARSING_STATE_INST,
132-
DXL2_0_PACKET_PARSING_STATE_ERROR,
133-
DXL2_0_PACKET_PARSING_STATE_PARAM,
134-
DXL2_0_PACKET_PARSING_STATE_CRC_L,
135-
DXL2_0_PACKET_PARSING_STATE_CRC_H
162+
DXL2_0_PACKET_PARSING_STATE_IDLE = 0,
163+
DXL2_0_PACKET_PARSING_STATE_RESERVED,
164+
DXL2_0_PACKET_PARSING_STATE_ID,
165+
DXL2_0_PACKET_PARSING_STATE_LENGTH_L,
166+
DXL2_0_PACKET_PARSING_STATE_LENGTH_H,
167+
DXL2_0_PACKET_PARSING_STATE_INST,
168+
DXL2_0_PACKET_PARSING_STATE_ERROR,
169+
DXL2_0_PACKET_PARSING_STATE_PARAM,
170+
DXL2_0_PACKET_PARSING_STATE_CRC_L,
171+
DXL2_0_PACKET_PARSING_STATE_CRC_H
136172
};
137173

138174
enum DXLLibErrorCode
@@ -161,6 +197,14 @@ enum DXLLibErrorCode
161197
DXL_LIB_ERROR_PORT_WRITE
162198
};
163199

200+
enum DXL_FastParamState {
201+
DXL_FAST_PARAM_ERROR = 0,
202+
DXL_FAST_PARAM_ID,
203+
DXL_FAST_PARAM_DATA,
204+
DXL_FAST_PARAM_CRC_L,
205+
DXL_FAST_PARAM_CRC_H
206+
};
207+
164208
typedef struct InfoToParseDXLPacket{
165209
uint8_t header[3];
166210
uint8_t header_cnt;
@@ -178,10 +222,12 @@ typedef struct InfoToParseDXLPacket{
178222
uint8_t recv_check_sum;
179223
uint8_t reserved;
180224
uint8_t parse_state;
181-
uint8_t param_count;
182225
uint8_t xel_count;
183-
// uint8_t check_xel_count;
184226
bool is_init;
227+
uint8_t xel_index;
228+
uint16_t buf_index;
229+
uint16_t param_length;
230+
uint8_t fast_param_state;
185231
}InfoToParseDXLPacket_t;
186232

187233
typedef struct InfoToMakeDXLPacket{
@@ -212,7 +258,7 @@ DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8
212258
DXLLibErrorCode_t fast_begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet,
213259
uint8_t protocol_ver);//, uint8_t* p_param_buf, uint16_t param_buf_cap, uint8_t xel_count);
214260
DXLLibErrorCode_t fast_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data,
215-
InfoFastSyncReadInst_t *sync_read, InfoFastBulkReadInst_t *bulk_read);
261+
InfoSyncReadInst_t *sync_read, InfoBulkReadInst_t *bulk_read);
216262

217263
#ifdef __cplusplus
218264
}

0 commit comments

Comments
 (0)