From e6886592029ec57f4249879011788bc9416284c4 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 27 Jun 2014 17:32:31 +0200
Subject: [PATCH] Added blockingReadAll
---
.../WeissHapticSensor/AbstractInterface.cpp | 12 +++++++++-
.../WeissHapticSensor/SerialInterface.cpp | 24 ++++++++++++++++++-
.../WeissHapticSensor/SerialInterface.h | 2 ++
.../WeissHapticSensor/WeissHapticSensor.cpp | 11 +++++++++
4 files changed, 47 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
index a81825709..d8cfc06a6 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
@@ -201,7 +201,7 @@ int AbstractInterface::receive( msg_t *msg )
//if ( !msg->data ) return -1;
// Read payload and checksum
- int maxReads = 10;
+ /*int maxReads = 10;
int remaining = msg->len + 2;
int dataOffset = 0;
while ( maxReads > 0 && remaining > 0)
@@ -216,9 +216,19 @@ int AbstractInterface::receive( msg_t *msg )
}
}
if (remaining > 0)
+ {
+ throw TransmissionException(str(boost::format("Not enough data (%d, expected %d), Command = %02X") % res % (msg->len + 2) % msg->id));
+ }*/
+
+ // Read payload and checksum:
+ // payload: msg->len
+ // checksum: 2 byte
+ int read = this->read( msg->data.data(), msg->len + 2 );
+ if (read != msg->len + 2)
{
throw TransmissionException(str(boost::format("Not enough data (%d, expected %d), Command = %02X") % res % (msg->len + 2) % msg->id));
}
+
/*
res = interface->read( msg->data, msg->len + 2 );
if ( res < (int) (msg->len + 2) )
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
index 77bc6b49f..ee19bf6cf 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
@@ -122,7 +122,7 @@ int SerialInterface::readInternal( unsigned char *buf, unsigned int len)
{
int res;
- res = ::read( fd, buf, len );
+ res = blockingReadAll( buf, len );
if ( res < 0 )
{
std::cerr << "Failed to read from serial device" << std::endl;
@@ -131,11 +131,33 @@ int SerialInterface::readInternal( unsigned char *buf, unsigned int len)
return res;
}
+
+int SerialInterface::blockingReadAll(unsigned char *buf, unsigned int len)
+{
+ int readData = 0;
+ while (1)
+ {
+ int res = ::read( fd, buf, len );
+ if (res < 0)
+ {
+ return res;
+ }
+ readData += res;
+ buf += res;
+ if (readData >= len)
+ {
+ return readData;
+ }
+ usleep(1);
+ }
+}
+
int SerialInterface::writeInternal( unsigned char *buf, unsigned int len)
{
return( ::write( fd, (void *) buf, len ) );
}
+
std::string SerialInterface::toString() const
{
return str(boost::format("SerialInterface(connected=%1%, device=%2%, bitrate=%3%, fd=%4%)")
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h
index a2bb216ce..c64e501d0 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h
@@ -23,6 +23,8 @@ private:
const char *device;
unsigned int bitrate;
int fd;
+
+ int blockingReadAll(unsigned char *buf, unsigned int len);
};
#endif // SERIALINTERFACE_H
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 82db8ae06..82beabb08 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -56,6 +56,8 @@ void WeissHapticSensor::connect()
sensor->setFrontEndGain(255);
+ ARMARX_LOG << "Front end gain set to " << (int)sensor->getFrontEndGain();
+
connected = true;
cout << this << ": Connect done, Interface=" << sensor->getInterfaceInfo() << endl;
}
@@ -89,9 +91,13 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
while(!sensorTask->isStopped())
{
//ARMARX_INFO << deactivateSpam(1) << this << ": receicePeriodicFrame";
+
try
{
+ //long start = TimestampVariant::nowLong();
PeriodicFrameData data = sensor->receicePeriodicFrame();
+ //long end = TimestampVariant::nowLong();
+ //cout << end - start << endl;
MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
for (int y = 0; y < mi.res_y; y++)
@@ -102,6 +108,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
(*matrix)(y, x) = val;
}
}
+
TimestampVariantPtr now = TimestampVariant::nowPtr();
writeMatrixToJs(matrix, now);
listenerPrx->reportSensorValues(device, tag, matrix, now);
@@ -110,6 +117,10 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
{
ARMARX_WARNING << "Caught ChecksumErrorException on " << device << ", skipping frame";
}
+ //usleep(3000);
+
+ //usleep(3000);
+ //usleep(1000000);
}
cout << this << ": stopPeriodicFrameAcquisition" << endl;
--
GitLab