From c0970c039f75b6689080dd013d9c19efbd3f603b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 19 May 2021 17:51:28 +0200
Subject: [PATCH] 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.
---
src/dynamixel_can.cpp | 36 +++++++++++++++++++-----------------
src/dynamixelserver_can.cpp | 20 ++++++++++----------
2 files changed, 29 insertions(+), 27 deletions(-)
diff --git a/src/dynamixel_can.cpp b/src/dynamixel_can.cpp
index 2fd46f5..f512a82 100644
--- a/src/dynamixel_can.cpp
+++ b/src/dynamixel_can.cpp
@@ -117,23 +117,23 @@ unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsig
if(this->comm_dev!=NULL)
{
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
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);
- 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)
{
- ((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;
}
else
{
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;
}
this->sync_packet_v1(data_int,read,&start);
@@ -142,19 +142,20 @@ unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsig
// read the remaining of the packet
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);
- 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)
{
- ((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;
}
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;
}
}
@@ -201,22 +202,22 @@ unsigned char CDynamixelCAN::receive_status_packet_v2(unsigned char **data,unsig
if(this->comm_dev!=NULL)
{
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
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);
- 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)
{
- ((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;
}
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;
}
this->sync_packet_v2(data_int,read,&start);
@@ -225,19 +226,20 @@ unsigned char CDynamixelCAN::receive_status_packet_v2(unsigned char **data,unsig
// read the remaining of the packet
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);
num=this->comm_dev->get_num_data();
}
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;
}
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;
}
}
diff --git a/src/dynamixelserver_can.cpp b/src/dynamixelserver_can.cpp
index 64259c1..26dcd56 100644
--- a/src/dynamixelserver_can.cpp
+++ b/src/dynamixelserver_can.cpp
@@ -155,13 +155,13 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data
if(this->comm_dev!=NULL)
{
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
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);
- 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)
{
@@ -179,10 +179,10 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data
// read the remaining of the packet
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);
- 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)
{
@@ -238,13 +238,13 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data
if(this->comm_dev!=NULL)
{
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
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);
- 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)
{
@@ -262,10 +262,10 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data
// read the remaining of the packet
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);
- 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)
{
--
GitLab