Skip to content
Snippets Groups Projects
Commit 5507b4bf authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved a bug in the dynamixel v2.0 protocol for the bulk read operation.

parent 330e7b51
No related branches found
No related tags found
No related merge requests found
......@@ -279,61 +279,61 @@ void CDynamixelServer::receive_sync_bulk_status_packets_v1(std::vector<unsigned
// read up to the length field
for(unsigned int i=0;i<servo_ids.size();)
{
if(need_data)
if(need_data)
{
this->event_server->wait_all(events,100);
new_data=this->comm_dev->get_num_data();
this->comm_dev->read(&data_int[total],new_data);
total+=new_data;
num+=new_data;
need_data=false;
total+=new_data;
num+=new_data;
need_data=false;
}
if(length==-1)
{
if(num>=4)
{
if(data_int[read+2]!=servo_ids[i])
if(length==-1)
{
if(num>=4)
{
if(data_int[read+2]!=servo_ids[i])
throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
length=data_int[read+3]+4;
}
else
need_data=true;
}
if(length!=-1 && num>=length)
{
}
else
need_data=true;
}
if(length!=-1 && num>=length)
{
if(this->compute_checksum_v1(&data_int[read],length)!=0x00)
{
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
read+=length;
num-=length;
length=-1;
i++;
continue;
}
try{
try{
this->handle_error(data_int[read+4]);
}catch(CException &e){
}catch(CException &e){
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
}
read+=length;
num-=length;
length=-1;
i++;
continue;
}
if(length>6)
{
data[i].resize(length-6);
for(unsigned j=0;j<(unsigned int)(length-6);j++)
data[i][j]=data_int[read+5+j];
for(unsigned j=0;j<(unsigned int)(length-6);j++)
data[i][j]=data_int[read+5+j];
}
else
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
}
else
read+=length;
num-=length;
length=-1;
i++;
}
else
need_data=true;
}
}catch(CEventTimeoutException &e){
......@@ -448,64 +448,64 @@ void CDynamixelServer::receive_sync_bulk_status_packets_v2(std::vector<unsigned
// read up to the length field
for(unsigned int i=0;i<servo_ids.size();)
{
if(need_data)
if(need_data)
{
this->event_server->wait_all(events,100);
new_data=this->comm_dev->get_num_data();
this->comm_dev->read(&data_int[total],new_data);
total+=new_data;
num+=new_data;
need_data=false;
total+=new_data;
num+=new_data;
need_data=false;
}
if(length==-1)
{
if(num>=7)
{
if(data_int[read+2]!=servo_ids[i])
if(length==-1)
{
if(num>=7)
{
if(data_int[read+4]!=servo_ids[i])
throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
length=data_int[read+5]+data_int[read+6]*256+7;
}
else
need_data=true;
}
if(length!=-1 && num>=length)
{
}
else
need_data=true;
}
if(length!=-1 && num>=length)
{
// check the checksum
crc=this->compute_checksum_v2(&data_int[read],length-2);
if((crc%256)!=data_int[read+length-2] || (crc/256)!=data_int[read+length-1])
{
/* handle exceptions */
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
read+=length;
num-=length;
length=-1;
i++;
continue;
}
try{
try{
this->handle_error(data_int[read+8]);
}catch(CException &e){
}catch(CException &e){
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
}
read+=length;
num-=length;
length=-1;
i++;
continue;
}
if(length>11)
{
data[i].resize(length-11);
for(unsigned j=0;j<(unsigned int)(length-11);j++)
data[i][j]=data_int[read+9+j];
for(unsigned j=0;j<(unsigned int)(length-11);j++)
data[i][j]=data_int[read+9+j];
}
else
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
}
else
read+=length;
num-=length;
length=-1;
i++;
}
else
need_data=true;
}
}catch(CEventTimeoutException &e){
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment