Added blockingReadAll

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) )

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%)")

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

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;