From c4d3a70547c29b6fe1d4408a79dc8b1b156744bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Mon, 2 Sep 2013 10:17:21 +0000 Subject: [PATCH] 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. --- src/dynamixel.cpp | 102 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 79 insertions(+), 23 deletions(-) diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp index b5449c0..7c85ff5 100644 --- a/src/dynamixel.cpp +++ b/src/dynamixel.cpp @@ -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; -- GitLab