Skip to content

Commit 78350dd

Browse files
honghyun79honghyun79
committed
Fast 프로토콜 구현
 - FastSyncRead 코드 수정 - FastBulkRead 추가
1 parent e68d9aa commit 78350dd

4 files changed

Lines changed: 289 additions & 174 deletions

File tree

‎src/dxl_c/protocol.cpp‎

Lines changed: 70 additions & 65 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);
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);
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,21 +349,19 @@ 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,
353-
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)//, uint8_t* p_param_buf, uint16_t param_buf_capacity, uint8_t xel_count)
354353
{
355-
if(param_buf_capacity > 0 && p_param_buf == NULL){
356-
return DXL_LIB_ERROR_NULLPTR;
357-
}
354+
// if(param_buf_capacity > 0 && p_param_buf == NULL){
355+
// return DXL_LIB_ERROR_NULLPTR;
356+
// }
358357

359-
if(protocol_ver != 1 && protocol_ver != 2){
358+
if (protocol_ver != 2)
360359
return DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION;
361-
}
362360

363361
p_parse_packet->protocol_ver = protocol_ver;
364-
p_parse_packet->p_param_buf = p_param_buf;
365-
p_parse_packet->param_buf_capacity = param_buf_capacity;
366-
p_parse_packet->xel_count = xel_count;
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;
367365
p_parse_packet->is_init = true;
368366

369367
return DXL_LIB_OK;
@@ -390,25 +388,19 @@ DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8
390388
return ret;
391389
}
392390

393-
DXLLibErrorCode_t fast_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data)
391+
DXLLibErrorCode_t fast_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data,
392+
InfoFastSyncReadInst_t *sync_read, InfoFastBulkReadInst_t *bulk_read)
394393
{
395-
DXLLibErrorCode_t ret;
396-
397-
if(p_parse_packet == NULL){
394+
if (p_parse_packet == NULL)
398395
return DXL_LIB_ERROR_NULLPTR;
399-
}else if(p_parse_packet->is_init == false){
396+
else if (p_parse_packet->is_init == false)
400397
return DXL_LIB_ERROR_NOT_INITIALIZED;
401-
}
402-
403-
if(p_parse_packet->protocol_ver == 2){
404-
ret = fast_parse_dxl2_0_packet(p_parse_packet, recv_data);
405-
}else if(p_parse_packet->protocol_ver == 1){
406-
ret = parse_dxl1_0_packet(p_parse_packet, recv_data);
407-
}else{
408-
ret = DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION;
409-
}
398+
else if (p_parse_packet->protocol_ver != 2)
399+
return DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION;
400+
else if ((nullptr == sync_read) && (nullptr == bulk_read))
401+
return DXL_LIB_ERROR_NULLPTR;
410402

411-
return ret;
403+
return fast_parse_dxl2_0_packet(p_parse_packet, recv_data, sync_read, bulk_read);
412404
}
413405

414406
static DXLLibErrorCode_t parse_dxl1_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data)
@@ -696,12 +688,10 @@ static DXLLibErrorCode_t parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_pac
696688
}
697689

698690
//fast_parse_dxl2_0_packet
699-
static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data)
691+
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)
700693
{
701694
DXLLibErrorCode_t ret = DXL_LIB_PROCEEDING;
702-
uint16_t byte_stuffing_cnt = 0;
703-
uint8_t size = ((p_parse_packet->param_buf_capacity+4) * p_parse_packet->xel_count) - 3; // 4 = Instruction(1)+Error(1)+CRC(2)
704-
uint8_t* param_array = (uint8_t *)malloc(sizeof(uint8_t) * size);
705695

706696
switch(p_parse_packet->parse_state)
707697
{
@@ -773,24 +763,25 @@ static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_pars
773763
p_parse_packet->err_idx = 0;
774764
p_parse_packet->recv_param_len = 0;
775765
p_parse_packet->param_count = 0;
776-
p_parse_packet->check_xel_count = 0;
766+
// p_parse_packet->check_xel_count = 0;
777767

778768
if(recv_data == DXL_INST_STATUS){
779769
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_ERROR;
780770
if(p_parse_packet->packet_len < 4){ // 4 = Instruction(1)+Error(1)+CRC(2)
781771
ret = DXL_LIB_ERROR_LENGTH;
782772
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE;
783-
}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)
784-
ret = DXL_LIB_ERROR_BUFFER_OVERFLOW;
785-
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;
786776
}else{
787777
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_ERROR;
788778
}
789779
}else{
790-
if(p_parse_packet->packet_len > p_parse_packet->param_buf_capacity+3){ // 3 = Instruction(1)+CRC(2)
791-
ret = DXL_LIB_ERROR_BUFFER_OVERFLOW;
792-
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE;
793-
}else if(p_parse_packet->packet_len == 3){ // 3 = Instruction(1)+CRC(2)
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
784+
if(p_parse_packet->packet_len == 3){ // 3 = Instruction(1)+CRC(2)
794785
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L;
795786
}else{
796787
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_PARAM;
@@ -809,38 +800,52 @@ static DXLLibErrorCode_t fast_parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_pars
809800
break;
810801

811802
case DXL2_0_PACKET_PARSING_STATE_PARAM:
812-
if(p_parse_packet->p_param_buf == NULL){
813-
ret = DXL_LIB_ERROR_NULLPTR;
814-
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE;
815-
}
816-
817-
param_array[p_parse_packet->recv_param_len] = recv_data;
818803
update_dxl_crc(&p_parse_packet->calculated_crc, recv_data);
819-
820-
821-
/////TODO/////
822804
// https://emanual.robotis.com/docs/en/dxl/protocol2/#parameter
823-
for(p_parse_packet->check_xel_count=0; p_parse_packet->check_xel_count < p_parse_packet->xel_count; p_parse_packet->check_xel_count++){
824-
if(p_parse_packet->check_xel_count == 0) {
825-
if(0 < p_parse_packet->recv_param_len && p_parse_packet->recv_param_len <= p_parse_packet->param_buf_capacity) {
826-
p_parse_packet->p_param_buf[p_parse_packet->param_count] = recv_data;
827-
p_parse_packet->param_count += 1;
828-
}
829-
} else {
830-
if((p_parse_packet->check_xel_count * (p_parse_packet->param_buf_capacity + 4)) < p_parse_packet->recv_param_len
831-
&& p_parse_packet->recv_param_len <= ((p_parse_packet->check_xel_count + 1) * p_parse_packet->param_buf_capacity) + 4) {
832-
p_parse_packet->p_param_buf[p_parse_packet->param_count] = recv_data;
833-
p_parse_packet->param_count += 1;
805+
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;
819+
}
820+
} 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;
834833
}
835834
}
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;
847+
}
836848
}
837-
838-
p_parse_packet->recv_param_len += 1;
839-
840-
if(p_parse_packet->recv_param_len == size) {
841-
p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L;
842-
}
843-
844849
break;
845850

846851
case DXL2_0_PACKET_PARSING_STATE_CRC_L:

‎src/dxl_c/protocol.h‎

Lines changed: 51 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,50 @@
2020
#include <stddef.h>
2121
#include <stdbool.h>
2222

23-
#define DXL_BROADCAST_ID 0xFE
23+
const int DXL_BROADCAST_ID = 0xFE;
2424

25+
namespace DYNAMIXEL {
26+
27+
typedef struct InfoSyncBulkBuffer{
28+
uint8_t* p_buf;
29+
uint16_t buf_capacity;
30+
uint16_t gen_length;
31+
bool is_completed;
32+
} __attribute__((packed)) InfoSyncBulkBuffer_t;
33+
34+
typedef struct XELInfoFastSyncRead{
35+
uint8_t *p_recv_buf;
36+
uint8_t id;
37+
uint8_t error;
38+
} __attribute__((packed)) XELInfoFastSyncRead_t;
39+
40+
typedef struct InfoFastSyncReadInst{
41+
uint16_t addr;
42+
uint16_t addr_length;
43+
XELInfoFastSyncRead_t* p_xels;
44+
uint8_t xel_count;
45+
bool is_info_changed;
46+
InfoSyncBulkBuffer_t packet;
47+
} __attribute__((packed)) InfoFastSyncReadInst_t;
48+
49+
typedef struct XELInfoFastBulkRead{
50+
uint16_t addr;
51+
uint16_t addr_length;
52+
uint8_t *p_recv_buf;
53+
uint8_t id;
54+
uint8_t error;
55+
} __attribute__((packed)) XELInfoFastBulkRead_t;
56+
57+
typedef struct InfoFastBulkReadInst{
58+
XELInfoFastBulkRead_t* p_xels;
59+
uint8_t xel_count;
60+
bool is_info_changed;
61+
InfoSyncBulkBuffer_t packet;
62+
} __attribute__((packed)) InfoFastBulkReadInst_t;
63+
64+
}
65+
66+
using namespace DYNAMIXEL;
2567

2668
#ifdef __cplusplus
2769
extern "C" {
@@ -43,6 +85,7 @@ enum DXLInstruction{
4385
DXL_INST_FAST_SYNC_READ = 0x8A, //ONLY Protocol2.0
4486
DXL_INST_SYNC_WRITE = 0x83,
4587
DXL_INST_BULK_READ = 0x92,
88+
DXL_INST_FAST_BULK_READ = 0x9A, // ONLY Protocol2.0
4689
DXL_INST_BULK_WRITE = 0x93 //ONLY Protocol2.0
4790
};
4891

@@ -137,7 +180,7 @@ typedef struct InfoToParseDXLPacket{
137180
uint8_t parse_state;
138181
uint8_t param_count;
139182
uint8_t xel_count;
140-
uint8_t check_xel_count;
183+
// uint8_t check_xel_count;
141184
bool is_init;
142185
}InfoToParseDXLPacket_t;
143186

@@ -153,7 +196,6 @@ typedef struct InfoToMakeDXLPacket{
153196
bool is_init;
154197
}InfoToMakeDXLPacket_t;
155198

156-
157199
typedef int32_t DXLLibErrorCode_t;
158200

159201
DXLLibErrorCode_t begin_make_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet,
@@ -163,16 +205,17 @@ DXLLibErrorCode_t add_param_to_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet,
163205
uint8_t *p_param, uint16_t param_len);
164206
DXLLibErrorCode_t end_make_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet);
165207

166-
167208
DXLLibErrorCode_t begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet,
168209
uint8_t protocol_ver, uint8_t* p_param_buf, uint16_t param_buf_cap);
169-
DXLLibErrorCode_t fast_begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet,
170-
uint8_t protocol_ver, uint8_t* p_param_buf, uint16_t param_buf_cap, uint8_t xel_count);
171210
DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data);
172-
DXLLibErrorCode_t fast_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data);
211+
212+
DXLLibErrorCode_t fast_begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet,
213+
uint8_t protocol_ver);//, uint8_t* p_param_buf, uint16_t param_buf_cap, uint8_t xel_count);
214+
DXLLibErrorCode_t fast_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data,
215+
InfoFastSyncReadInst_t *sync_read, InfoFastBulkReadInst_t *bulk_read);
173216

174217
#ifdef __cplusplus
175218
}
176219
#endif
177220

178-
#endif /* DYNAMIXEL_PROTOCOL_H_ */
221+
#endif /* DYNAMIXEL_PROTOCOL_H_ */

0 commit comments

Comments
 (0)