CDxlSerialPacketHandler.cpp
Go to the documentation of this file.
00001 #include <threemxl/platform/hardware/dynamixel/CDxlCom.h>
00002 #include <threemxl/platform/hardware/dynamixel/CDxlSerialPacketHandler.h>
00003 
00004 CDxlSerialPacketHandler::CDxlSerialPacketHandler(LxSerial *&serialPort) :
00005   mLog("CDxlCom"),
00006   mSerialPort(serialPort),
00007   mLastError(0)
00008 {
00009 }
00010 
00011 int CDxlSerialPacketHandler::init()
00012 {
00013   if (mSerialPort == NULL)
00014     return DXL_NO_SERIAL_PORT;
00015     
00016   return DXL_SUCCESS;
00017 }
00018 
00019 int CDxlSerialPacketHandler::sendPacket(CDxlPacket *packet, bool replyExpected)
00020 {
00021         int numBytesWritten = DXL_ERROR;
00022         for (int i =0 ; i<=SEND_RETRY_FACTOR; i++)
00023         {
00024                 mLogCrawlLn("Sending packet " << packet->getPktString() << " try " << i );
00025                 numBytesWritten = mSerialPort->port_write(packet->data(), packet->length());
00026                 if (numBytesWritten == packet->length())
00027                 {
00028                         break; //i=SEND_RETRY_FACTOR + 1; // we are done and exit this loop
00029                 }
00030                 else
00031                 {
00032                         //we did not correctly send the packet so we do a cleanup
00033                         if (numBytesWritten < 0)
00034                                 mLogWarningLn("Sending packet failed with error code " << -numBytesWritten << ")...");
00035                         else
00036                                 mLogWarningLn("Sending packet failed, only written " << numBytesWritten <<" chars...");
00037 
00038                         mSerialPort->flush_buffer();
00039                 }
00040 
00041         }
00042 
00043         if (numBytesWritten != packet->length())
00044         {
00045                 mLogErrorLn("sendPacket() failed permanently after "<< SEND_RETRY_FACTOR+1 <<" tries!");
00046                 mLastError = numBytesWritten;
00047                 return DXL_PKT_SEND_ERROR;
00048         }
00049 
00050         return DXL_SUCCESS;
00051 }
00052 
00053 int CDxlSerialPacketHandler::receivePacketWait(CDxlStatusPacket *packet, int seconds, int microseconds)
00054 {
00055         int numBytesRead = DXL_ERROR;
00056 //      mLogCrawlLn("waiting for packet with length "<< (int)packet->length());
00057         for (int i =0 ; i<=RECEIVE_RETRY_FACTOR; i++)
00058         {
00059                 numBytesRead = mSerialPort->port_read(packet->data(), packet->length(), seconds, microseconds);
00060                 if (numBytesRead == packet->length())
00061                 {
00062                         int s = 0;
00063                         
00064                         // Strip leading bytes before packet start
00065                         while (s < packet->length()-1 && (packet->data()[s] != 0xff || packet->data()[s+1] != 0xff)) s++;
00066                         
00067                         // Strip leading ffs before packet start
00068                         while (s < packet->length()-2 && packet->data()[s+2] == 0xff) s++;
00069                         
00070                         if (s)
00071                         {
00072                                 // We removed some characters. Try to get rest of packet now
00073                                 memmove(packet->data(), packet->data()+s, packet->length()-s);
00074                                 int n = mSerialPort->port_read(packet->data()+packet->length()-s, s, seconds, microseconds);
00075                                 if (n != s)
00076                                 {
00077                                         mLogErrorLn("Error getting rest of mangled packet " << packet->getPktString());
00078                                         mSerialPort->flush_buffer();
00079                                         usleep(10);
00080                                         mSerialPort->flush_buffer();
00081                                         if (n < 0)
00082                                                 mLastError = n;
00083                                 }
00084                                 else
00085                                 {
00086                                         mLogCrawlLn("Received initially mangled packet " << packet->getPktString() << " try " << i );
00087                                         break; //i = RECEIVE_RETRY_FACTOR + 1; // we are done and exit this loop
00088                                 }
00089                         }
00090                         else
00091                         {
00092                                 mLogCrawlLn("Received packet " << packet->getPktString() << " try " << i );
00093                                 break; //i = RECEIVE_RETRY_FACTOR + 1; // we are done and exit this loop
00094                         }
00095                 }
00096                 else
00097                 {
00098                         if (numBytesRead < 0 )
00099                         {
00100                                 mLastError = numBytesRead;
00101                                 if (errno)
00102                                 {
00103                                         mLogErrorLn("Receiving packet failed... with error code \""<< strerror(errno) << "\"(" << errno << ")..." );
00104                                         mSerialPort->flush_buffer(); //we want to flush only if it was not a timeout
00105                                         usleep(10);                                      //we waitpacket->length()
00106                                         mSerialPort->flush_buffer(); //we want to flush again
00107                                 }
00108                                 else
00109                                         mLogDebugLn("Timeout waiting for packet");
00110                         }
00111                         else
00112                         {
00113                                 mLogErrorLn("Received packet with different length (" << numBytesRead << " in stead of " << (int)packet->length() << ") " << packet->getPktString(numBytesRead) << " try " << i );
00114                         }
00115                 }
00116         }
00117 
00118 
00119 
00120 
00121 //      if (numBytesRead < 0 )
00122 //      {
00123 //              mLastError = numBytesRead;
00124 //              return DXL_PKT_RECV_ERROR;
00125 //      }
00126 //      else
00127         if (numBytesRead != packet->length())
00128         {
00129 //              mLogErrorLn("receivePacketWait() failed permanently after " << RECEIVE_RETRY_FACTOR+1 << " try/tries!");
00130                 mLastError = numBytesRead;
00131                 return DXL_PKT_RECV_ERROR;
00132         }
00133 
00134         if (packet->calcChecksum() != packet->readChecksum())
00135         {
00136 //              #ifdef __DBG__
00137 //              printf("warning: checksum error in dxl package. RecvCS: %02X CalcCS: %02X\n",packet->readChecksum(), packet->calcChecksum());
00138 //              #endif
00139                 mLogErrorLn("Checksum error in packet: Received CS:"<< std::setw (2) << std::uppercase << std::hex << (int)packet->readChecksum() << " Calculated CS:"<< std::hex << (int)packet->calcChecksum() << std::dec);
00140                 mLogErrorLn("Packet was " << packet->getPktString());
00141                 mLogCrawlLn("flush buffers");
00142                 mSerialPort->flush_buffer();
00143                 mLastError = numBytesRead;
00144                 return DXL_PKT_RECV_CHECKSUM_ERR;
00145         }
00146 
00147         return DXL_SUCCESS;
00148 }
00149 
00150 int CDxlSerialPacketHandler::getLastError()
00151 {
00152   return mLastError;
00153 }
00154 


threemxl
Author(s):
autogenerated on Fri Aug 28 2015 13:21:08