From 6716b06fde9edb28e41c024baa0df98a71ab3021 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Sat, 2 Apr 2016 17:48:10 +0000
Subject: [PATCH] Unblocked the mutex when calling the wait_all() function.
---
src/dynamixel.cpp | 1 -
src/dynamixelserver.cpp | 10 +++++++++-
2 files changed, 9 insertions(+), 2 deletions(-)
diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 7ddd947..61df2a8 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -290,7 +290,6 @@ void CDynamixel::handle_error(unsigned char error)
error_msg+="\nOverload";
if(error&0x40)
error_msg+="\nInvalid instruction";
- std::cout << error_msg << std::endl;
throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,error);
}
}
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index 822a9c0..c48e6a8 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -194,7 +194,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
+ this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
+ this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
@@ -215,7 +217,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
{
if((num=this->comm_dev->get_num_data())==0)
{
+ this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
+ this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read-start+num)>length)
@@ -277,7 +281,9 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
+ this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
+ this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
@@ -291,14 +297,16 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
read+=num;
}
this->sync_packet_v2(data_int,read,&start);
- }while(read<7);
+ }while((read-start)<7);
length=data_int[start+5]+data_int[start+6]*256+7;
// read the remaining of the packet
while((read-start)<length)
{
if((num=this->comm_dev->get_num_data())==0)
{
+ this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
+ this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read-start+num)>length)
--
GitLab