Commit 0bf31506 authored by Irene Garcia Camacho's avatar Irene Garcia Camacho

Removed most of the dynamic memory for testing.

parent bc2bcfde
......@@ -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;
......
Markdown is supported
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