From 9b2d34e15dd176b0432533b3ab790c465665688a Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Sep 2020 12:59:17 +0200
Subject: [PATCH] Solved some bugs in the protocol version conversion
functions.
---
dynamixel_base/src/dynamixel.c | 23 +++++++++++++++++------
dynamixel_base/src/dynamixel2.c | 26 ++++++++++++++++++++------
2 files changed, 37 insertions(+), 12 deletions(-)
diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c
index e25fdb7..8c00138 100755
--- a/dynamixel_base/src/dynamixel.c
+++ b/dynamixel_base/src/dynamixel.c
@@ -51,10 +51,21 @@ unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *d
destination[DYN_HEADER_OFF]=0xFF;
destination[DYN_HEADER_OFF+1]=0xFF;
destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
- destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+ destination[DYN_LENGTH_OFF]=2;//source[DYN2_LENGTH_OFF];
destination[DYN_INST_OFF]=source[DYN2_INST_OFF];
- for(i=0;i<destination[DYN_LENGTH_OFF];i++)
- destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+ switch(source[DYN2_INST_OFF])
+ {
+ case DYN_PING: break;
+ case DYN_READ: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
+ destination[DYN_DATA_OFF+1]=source[DYN2_DATA_OFF+2];
+ destination[DYN_LENGTH_OFF]+=2;
+ break;
+ case DYN_WRITE: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
+ for(i=0;i<source[DYN2_DATA_OFF]-5;i++)
+ destination[DYN_DATA_OFF+1+i]=source[DYN2_DATA_OFF+2+i];
+ destination[DYN_LENGTH_OFF]+=source[DYN2_DATA_OFF]-4;
+ break;
+ }
dyn_set_checksum(destination);
return 0x01;
}
@@ -247,10 +258,10 @@ unsigned char dyn_convert_v2_status_packet(unsigned char *source, unsigned char
destination[DYN_HEADER_OFF]=0xFF;
destination[DYN_HEADER_OFF+1]=0xFF;
destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
- destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+ destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF]-2;
destination[DYN_ERROR_OFF]=source[DYN2_ERROR_OFF];
- for(i=0;i<destination[DYN_LENGTH_OFF];i++)
- destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+ for(i=0;i<destination[DYN_LENGTH_OFF]-2;i++)
+ destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+1+i];
dyn_set_checksum(destination);
return 0x01;
}
diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c
index dc26232..ce34b11 100644
--- a/dynamixel_base/src/dynamixel2.c
+++ b/dynamixel_base/src/dynamixel2.c
@@ -88,11 +88,25 @@ unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *
destination[DYN2_HEADER_OFF+2]=0xFD;
destination[DYN2_RESERVED]=0x00;
destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
- destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+ destination[DYN2_LENGTH_OFF]=3;//source[DYN_LENGTH_OFF];
destination[DYN2_LENGTH_OFF+1]=0x00;
destination[DYN2_INST_OFF]=source[DYN_INST_OFF];
- for(i=0;i<source[DYN_LENGTH_OFF];i++)
- destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+ switch(source[DYN_INST_OFF])
+ {
+ case DYN_PING: break;
+ case DYN_READ: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
+ destination[DYN2_DATA_OFF+1]=0x00;
+ destination[DYN2_DATA_OFF+2]=source[DYN_DATA_OFF+1];
+ destination[DYN2_DATA_OFF+3]=0x00;
+ (*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=4;
+ break;
+ case DYN_WRITE: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
+ destination[DYN2_DATA_OFF+1]=0x00;
+ for(i=0;i<source[DYN_LENGTH_OFF]-3;i++)
+ destination[DYN2_DATA_OFF+2+i]=source[DYN_DATA_OFF+1+i];
+ (*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=source[DYN_LENGTH_OFF]-1;
+ break;
+ }
dyn2_set_checksum(destination);
return 0x01;
@@ -417,12 +431,12 @@ unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *
destination[DYN2_HEADER_OFF+2]=0xFD;
destination[DYN2_RESERVED]=0x00;
destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
- destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+ destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF]+2;
destination[DYN2_LENGTH_OFF+1]=0x00;
destination[DYN2_INST_OFF]=inst;
destination[DYN2_ERROR_OFF]=source[DYN_ERROR_OFF];
- for(i=0;i<source[DYN_LENGTH_OFF];i++)
- destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+ for(i=0;i<destination[DYN2_LENGTH_OFF]-4;i++)
+ destination[DYN2_DATA_OFF+1+i]=source[DYN_DATA_OFF+i];
dyn2_set_checksum(destination);
return 0x01;
--
GitLab