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

Solved a bug in the reception of data from thge servos. When additionalbytes...

Solved a bug in the reception of data from thge servos. When additionalbytes were receveid due to errors in the communication link, memory leaks appeared.
parent 544638d7
No related branches found
No related tags found
No related merge requests found
......@@ -112,9 +112,9 @@ void CDynamixel::resync(void)
void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
{
unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
int num=0,read=0,it=0,read_length=7;
int num=0,read=0,it=0,read_length=7,i=0;
std::list<std::string> events;
unsigned char data[7];
unsigned char data[7],dummy;
std::string error_msg;
bool rx_tx_ok=false;
......@@ -142,7 +142,14 @@ void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&data[read],num);
if(read+num>read_length)
{
this->usb_dev->read(&data[read],read_length-read);
for(i=0;i<(num-read_length+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&data[read],num);
read+=num;
if(read>5 && data[4]!=0x00)
read_length=6;
......@@ -203,9 +210,9 @@ void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
void CDynamixel::read_word_register(unsigned char address,unsigned short int *value)
{
unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
int num=0,read=0,it=0,read_length=8;
int num=0,read=0,it=0,read_length=8,i=0;
std::list<std::string> events;
unsigned char data[8];
unsigned char data[8],dummy;
std::string error_msg;
bool rx_tx_ok=false;
......@@ -233,7 +240,14 @@ void CDynamixel::read_word_register(unsigned char address,unsigned short int *va
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&data[read],num);
if(read+num>read_length)
{
this->usb_dev->read(&data[read],read_length-read);
for(i=0;i<(num-read_length+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&data[read],num);
read+=num;
if(read>5 && data[4]!=0x00)
read_length=6;
......@@ -295,9 +309,9 @@ void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
{
unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x03,0x00,0x00,0x00};
std::list<std::string> events;
unsigned char answer[6];
unsigned char answer[6],dummy;
int num=0,read=0,it=0,i=0;
std::string error_msg;
int num=0,read=0,it=0;
bool rx_tx_ok=false;
if(this->usb_dev!=NULL)
......@@ -324,7 +338,14 @@ void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&answer[read],num);
if(read+num>6)
{
this->usb_dev->read(&answer[read],6-read);
for(i=0;i<(num-6+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -379,9 +400,9 @@ void CDynamixel::write_word_register(unsigned char address, unsigned short int d
{
unsigned char packet[9]={0xFF,0xFF,0x00,0x05,0x03,0x00,0x00,0x00,0x00};
std::list<std::string> events;
unsigned char answer[6];
unsigned char answer[6],dummy;
int num=0,read=0,it=0,i=0;
std::string error_msg;
int num=0,read=0,it=0;
bool rx_tx_ok=false;
if(this->usb_dev!=NULL)
......@@ -409,7 +430,14 @@ void CDynamixel::write_word_register(unsigned char address, unsigned short int d
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&answer[read],num);
if(read+num>6)
{
this->usb_dev->read(&answer[read],6-read);
for(i=0;i<(num-6+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -464,9 +492,9 @@ void CDynamixel::registered_byte_write(unsigned char address, unsigned char data
{
unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x04,0x00,0x00,0x00};
std::list<std::string> events;
unsigned char answer[6];
unsigned char answer[6],dummy;
int num=0,read=0,it=0,i=0;
std::string error_msg;
int num=0,read=0,it=0;
bool rx_tx_ok=false;
if(this->usb_dev!=NULL)
......@@ -493,7 +521,14 @@ void CDynamixel::registered_byte_write(unsigned char address, unsigned char data
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&answer[read],num);
if(read+num>6)
{
this->usb_dev->read(&answer[read],6-read);
for(i=0;i<(num-6+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -548,9 +583,9 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
{
unsigned char packet[9]={0xFF,0xFF,0x00,0x05,0x04,0x00,0x00,0x00,0x00};
std::list<std::string> events;
unsigned char answer[6];
unsigned char answer[6],dummy;
int num=0,read=0,it=0,i=0;
std::string error_msg;
int num=0,read=0,it=0;
bool rx_tx_ok=false;
if(this->usb_dev!=NULL)
......@@ -578,7 +613,14 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&answer[read],num);
if(read+num>6)
{
this->usb_dev->read(&answer[read],6-read);
for(i=0;i<(num-6+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -633,9 +675,9 @@ void CDynamixel::write_registers(unsigned char address, unsigned char *data, uns
{
unsigned char *packet;
std::list<std::string> events;
unsigned char answer[6];
std::string error_msg;
unsigned char answer[6],dummy;
int num=0,read=0,it=0;
std::string error_msg;
bool rx_tx_ok=false;
unsigned int i=0;
......@@ -678,7 +720,14 @@ void CDynamixel::write_registers(unsigned char address, unsigned char *data, uns
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&answer[read],num);
if(read+num>6)
{
this->usb_dev->read(&answer[read],6-read);
for(i=0;i<(num-6+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -735,7 +784,7 @@ void CDynamixel::read_registers(unsigned char address, unsigned char *data, unsi
unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
int num=0,read=0,it=0,read_length=length+6;
std::list<std::string> events;
unsigned char *answer;
unsigned char *answer,dummy;
std::string error_msg;
bool rx_tx_ok=false;
unsigned int i=0;
......@@ -765,7 +814,14 @@ void CDynamixel::read_registers(unsigned char address, unsigned char *data, unsi
{
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
this->usb_dev->read(&answer[read],num);
if(read+num>read_length)
{
this->usb_dev->read(&answer[read],read_length-read);
for(i=0;i<(num-read_length+read);i++)
this->usb_dev->read(&dummy,1);
}
else
this->usb_dev->read(&answer[read],num);
read+=num;
if(read>5 && answer[4]!=0x00)
read_length=6;
......
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