Skip to content
Snippets Groups Projects
Commit 0bf31506 authored by Irene Garcia Camacho's avatar Irene Garcia Camacho
Browse files

Removed most of the dynamic memory for testing.

parent bc2bcfde
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 7 additions and 27 deletions
src/dynamixel_slave.cpp
+ 7
− 27
View file @ 0bf31506
......@@ -31,7 +31,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
CDynamixelSlave *slave=(CDynamixelSlave *)params;
std::list<std::string> events;
int event_index,num,i;
unsigned char *data,*new_data;
unsigned char data[256],new_data[256];
bool end=false;
static bool data_phase=false;
......@@ -59,7 +59,6 @@ void *CDynamixelSlave::process_packets_thread(void *params)
// process the incomming data
slave->comm_access.enter();
num=slave->comm_dev->get_num_data();
data=new unsigned char[num];
if(slave->comm_dev->read(data,num)!=num)
{
slave->comm_access.exit();
......@@ -139,7 +138,6 @@ void *CDynamixelSlave::process_packets_thread(void *params)
if(length==0)
{
data_phase=false;
new_data=new unsigned char[num_bytes];
memcpy(new_data,packet,num_bytes);
slave->packets.push_back(new_data);
if(slave->dyn_ver==dyn_version1)
......@@ -153,8 +151,6 @@ void *CDynamixelSlave::process_packets_thread(void *params)
}
}
}
delete[] data;
data=NULL;
}
pthread_exit(NULL);
......@@ -214,7 +210,7 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
{
unsigned short int length,address;
unsigned char error,id,prev_id;;
unsigned char *data,*write_data,*read_data;
unsigned char *data,*write_data,read_data[256];
unsigned int i=0;
data=this->packets[0];
......@@ -233,7 +229,8 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
if(id!=0xFE)
this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
break;
case dyn_read: read_data=new unsigned char[this->get_read_length(data)];
case dyn_read: std::cout << "length: " << this->get_read_length(data) << std::endl;
std::cout << "address: " << this->get_read_address(data) << std::endl;
error=this->slaves[i].on_read(this->get_read_address(data),this->get_read_length(data),read_data);
if(this->return_level!=no_return && id!=0xFE)
{
......@@ -242,7 +239,6 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
}
delete[] read_data;
break;
case dyn_write: length=this->get_write_data(data,&write_data);
error=this->slaves[i].on_write(this->get_write_address(data),length,write_data);
......@@ -280,13 +276,11 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
{
if(prev_id==0x00)// first device to answer
{
read_data=new unsigned char[length];
error=this->slaves[i].on_read(address,length,read_data);
if(error==dyn_no_error)
this->send_status_packet(this->slaves[i].id,dyn_no_error,length,read_data);
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
delete[] read_data;
}
else// wait for the previous device in the sequence to send its data
{
......@@ -317,13 +311,11 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
if(id==this->slaves[i].bulk_prev_id)
{
this->slaves[i].bulk_read_pending=false;
read_data=new unsigned char[this->slaves[i].bulk_length];
error=this->slaves[i].on_read(this->slaves[i].bulk_address,this->slaves[i].bulk_length,read_data);
if(error==dyn_no_error)
this->send_status_packet(this->slaves[i].id,dyn_no_error,this->slaves[i].bulk_length,read_data);
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
delete[] read_data;
}
}
}
......@@ -335,7 +327,7 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
{
unsigned char error,prev_id,id;
unsigned short int length,address,checksum,checksum_pkt;
unsigned char *data,*write_data,*read_data;
unsigned char *data,*write_data,read_data[256];
unsigned int i=0;
data=this->packets[0];
......@@ -356,8 +348,7 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
if(id!=0xFE)
this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
break;
case dyn_read: read_data=new unsigned char[this->get_read_length(data)];
error=this->slaves[i].on_read(this->get_read_address(data),this->get_read_length(data),read_data);
case dyn_read: error=this->slaves[i].on_read(this->get_read_address(data),this->get_read_length(data),read_data);
if(this->return_level!=no_return && id!=0xFE)
{
if(error==dyn_no_error)
......@@ -365,7 +356,6 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
}
delete[] read_data;
break;
case dyn_write: length=this->get_write_data(data,&write_data);
error=this->slaves[i].on_write(this->get_write_address(data),length,write_data);
......@@ -398,13 +388,11 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
{
if(prev_id==0x00)// first device to answer
{
read_data=new unsigned char[length];
error=this->slaves[i].on_read(address,length,read_data);
if(error==dyn_no_error)
this->send_status_packet(this->slaves[i].id,dyn_no_error,length,read_data);
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
delete[] read_data;
}
else// wait for the previous device in the sequence to send its data
{
......@@ -423,13 +411,11 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
{
if(prev_id==0x00)// first device to answer
{
read_data=new unsigned char[length];
error=this->slaves[i].on_read(address,length,read_data);
if(error==dyn_no_error)
this->send_status_packet(this->slaves[i].id,dyn_no_error,length,read_data);
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
delete read_data;
}
else// wait for the previous device in the sequence to send its data
{
......@@ -462,13 +448,11 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
if(id==this->slaves[i].bulk_prev_id)
{
this->slaves[i].bulk_read_pending=false;
read_data=new unsigned char[this->slaves[i].bulk_length];
error=this->slaves[i].on_read(this->slaves[i].bulk_address,this->slaves[i].bulk_length,read_data);
if(error==dyn_no_error)
this->send_status_packet(this->slaves[i].id,dyn_no_error,this->slaves[i].bulk_length,read_data);
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
delete read_data;
}
}
else if(this->slaves[i].sync_read_pending)
......@@ -476,13 +460,11 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
if(id==this->slaves[i].sync_prev_id)
{
this->slaves[i].sync_read_pending=false;
read_data=new unsigned char[this->slaves[i].sync_length];
error=this->slaves[i].on_read(this->slaves[i].sync_address,this->slaves[i].sync_length,read_data);
if(error==dyn_no_error)
this->send_status_packet(this->slaves[i].id,dyn_no_error,this->slaves[i].sync_length,read_data);
else
this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
delete[] read_data;
}
}
}
......@@ -742,7 +724,7 @@ bool CDynamixelSlave::bulk_write_id_present(unsigned char *data,unsigned char id
// status return
void CDynamixelSlave::send_status_packet(unsigned char id,dyn_error_t error,unsigned short int length,unsigned char *data)
{
unsigned char *status_data;
unsigned char status_data[256];
unsigned short int checksum;
if(this->return_delay>0)
......@@ -751,7 +733,6 @@ void CDynamixelSlave::send_status_packet(unsigned char id,dyn_error_t error,unsi
if(this->dyn_ver==dyn_version1)
{
this->comm_access.enter();
status_data=new unsigned char[6+length];
status_data[0]=0xFF;
status_data[1]=0xFF;
status_data[2]=id;
......@@ -767,7 +748,6 @@ void CDynamixelSlave::send_status_packet(unsigned char id,dyn_error_t error,unsi
else
{
this->comm_access.enter();
status_data=new unsigned char[11+length];
status_data[0]=0xFF;
status_data[1]=0xFF;
status_data[2]=0xFD;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment