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

Used the specific new data event for the rx can ID. 

Checked that the number of data available if not 0 before reading to avoid an exception.
parent 6b2ff973
No related branches found
No related tags found
1 merge request!2Used the specific new data event for the rx can ID.
Hide whitespace changes
Inline Side-by-side
src/dynamixel_can.cpp
+ 19
− 17
View file @ c0970c03
...@@ -117,23 +117,23 @@ unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsig ...@@ -117,23 +117,23 @@ unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsig
if(this->comm_dev!=NULL) if(this->comm_dev!=NULL)
{ {
try{ try{
events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id()); events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id(this->rx_frame_id));
// read up to the length field // read up to the length field
do{ do{
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id))==0)
{ {
this->event_server->wait_all(events,500); this->event_server->wait_all(events,500);
num=((CCAN *)this->comm_dev)->get_num_bytes(rx_frame_id); num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id);
} }
if((read+num)>1024) if((read+num)>1024)
{ {
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],1024-read); ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],1024-read);
read=1024; read=1024;
} }
else else
{ {
if(num != 0) if(num != 0)
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],num); ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],num);
read+=num; read+=num;
} }
this->sync_packet_v1(data_int,read,&start); this->sync_packet_v1(data_int,read,&start);
...@@ -142,19 +142,20 @@ unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsig ...@@ -142,19 +142,20 @@ unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsig
// read the remaining of the packet // read the remaining of the packet
while((read-start)<length) while((read-start)<length)
{ {
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id))==0)
{ {
this->event_server->wait_all(events,500); this->event_server->wait_all(events,500);
num=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id); num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id);
} }
if((read-start+num)>length) if((read-start+num)>length)
{ {
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],length-read); ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],length-read);
read=length; read=length;
} }
else else
{ {
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],num); if(num != 0)
((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],num);
read+=num; read+=num;
} }
} }
...@@ -201,22 +202,22 @@ unsigned char CDynamixelCAN::receive_status_packet_v2(unsigned char **data,unsig ...@@ -201,22 +202,22 @@ unsigned char CDynamixelCAN::receive_status_packet_v2(unsigned char **data,unsig
if(this->comm_dev!=NULL) if(this->comm_dev!=NULL)
{ {
try{ try{
events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id()); events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id(this->rx_frame_id));
// read up to the length field // read up to the length field
do{ do{
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id))==0)
{ {
this->event_server->wait_all(events,100); this->event_server->wait_all(events,100);
num=((CCAN *)this->comm_dev)->get_num_bytes(rx_frame_id); num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id);
} }
if((read+num)>1024) if((read+num)>1024)
{ {
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],1024-read); ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],1024-read);
read=1024; read=1024;
} }
else else
{ {
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],num); ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],num);
read+=num; read+=num;
} }
this->sync_packet_v2(data_int,read,&start); this->sync_packet_v2(data_int,read,&start);
...@@ -225,19 +226,20 @@ unsigned char CDynamixelCAN::receive_status_packet_v2(unsigned char **data,unsig ...@@ -225,19 +226,20 @@ unsigned char CDynamixelCAN::receive_status_packet_v2(unsigned char **data,unsig
// read the remaining of the packet // read the remaining of the packet
while((read-start)<length) while((read-start)<length)
{ {
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id))==0)
{ {
this->event_server->wait_all(events,100); this->event_server->wait_all(events,100);
num=this->comm_dev->get_num_data(); num=this->comm_dev->get_num_data();
} }
if((read-start+num)>1024) if((read-start+num)>1024)
{ {
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],1024-read); ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],1024-read);
read=1024; read=1024;
} }
else else
{ {
((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],num); if(num != 0)
((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],num);
read+=num; read+=num;
} }
} }
......
src/dynamixelserver_can.cpp
+ 10
− 10
View file @ c0970c03
...@@ -155,13 +155,13 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data ...@@ -155,13 +155,13 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data
if(this->comm_dev!=NULL) if(this->comm_dev!=NULL)
{ {
try{ try{
events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id()); events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id(this->bus_info.rx_frame_id));
// read up to the length field // read up to the length field
do{ do{
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id))==0)
{ {
this->event_server->wait_all(events,200); this->event_server->wait_all(events,200);
num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id); num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id);
} }
if((read+num)>1024) if((read+num)>1024)
{ {
...@@ -179,10 +179,10 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data ...@@ -179,10 +179,10 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data
// read the remaining of the packet // read the remaining of the packet
while((read-start)<length) while((read-start)<length)
{ {
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id))==0)
{ {
this->event_server->wait_all(events,200); this->event_server->wait_all(events,200);
num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id); num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id);
} }
if((read-start+num)>length) if((read-start+num)>length)
{ {
...@@ -238,13 +238,13 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data ...@@ -238,13 +238,13 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data
if(this->comm_dev!=NULL) if(this->comm_dev!=NULL)
{ {
try{ try{
events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id()); events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id(this->bus_info.rx_frame_id));
// read up to the length field // read up to the length field
do{ do{
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id))==0)
{ {
this->event_server->wait_all(events,200); this->event_server->wait_all(events,200);
num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id); num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id);
} }
if((read+num)>1024) if((read+num)>1024)
{ {
...@@ -262,10 +262,10 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data ...@@ -262,10 +262,10 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data
// read the remaining of the packet // read the remaining of the packet
while((read-start)<length) while((read-start)<length)
{ {
if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id))==0) if((num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id))==0)
{ {
this->event_server->wait_all(events,200); this->event_server->wait_all(events,200);
num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id); num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id);
} }
if((read-start+num)>length) if((read-start+num)>length)
{ {
......
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