SVHSerialInterface.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK SVH Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2014 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 //
00014 // -- END LICENSE BLOCK ------------------------------------------------
00015 
00016 //----------------------------------------------------------------------
00029 //----------------------------------------------------------------------
00030 #include "driver_svh/SVHSerialInterface.h"
00031 #include "driver_svh/Logging.h"
00032 
00033 #include <icl_comm/ByteOrderConversion.h>
00034 
00035 using icl_core::TimeSpan;
00036 using icl_comm::serial::SerialFlags;
00037 
00038 namespace driver_svh {
00039 
00040 SVHSerialInterface::SVHSerialInterface(ReceivedPacketCallback const & received_packet_callback) :
00041   m_connected(false),
00042   m_received_packet_callback(received_packet_callback),
00043   m_packets_transmitted(0)
00044 {
00045 }
00046 
00047 SVHSerialInterface::~SVHSerialInterface()
00048 {
00049   //close();
00050 }
00051 
00052 bool SVHSerialInterface::connect(const std::string &dev_name)
00053 {
00054   // close device if already opened
00055   close();
00056 
00057   // create serial device
00058   m_serial_device.reset(new Serial(dev_name.c_str(), SerialFlags(SerialFlags::eBR_921600, SerialFlags::eDB_8)));
00059 
00060   if (m_serial_device)
00061   {
00062     // open serial device
00063     if (!m_serial_device->Open())
00064     {
00065       LOGGING_ERROR_C(DriverSVH, SVHSerialInterface, "Could not open serial device: " << dev_name.c_str() << endl);
00066       return false;
00067     }
00068   }
00069   else
00070   {
00071     LOGGING_ERROR_C(DriverSVH, SVHSerialInterface, "Could not create serial device handle: " << dev_name.c_str() << endl);
00072     return false;
00073   }
00074 
00075   // create receive thread
00076   m_receive_thread.reset(new SVHReceiveThread(TimeSpan(0, 500000), m_serial_device, m_received_packet_callback));
00077 
00078   if (m_receive_thread)
00079   {
00080     // start receive thread
00081     if (!m_receive_thread->start())
00082     {
00083       LOGGING_ERROR_C(DriverSVH, SVHSerialInterface, "Could not start the receive thread for the serial device!" << endl);
00084       return false;
00085     }
00086   }
00087   else
00088   {
00089     LOGGING_ERROR_C(DriverSVH, SVHSerialInterface, "Could not create the receive thread for the serial device!" << endl);
00090     return false;
00091   }
00092 
00093   m_connected = true;
00094   LOGGING_TRACE_C(DriverSVH, SVHSerialInterface, "Serial device  " << dev_name.c_str()  << " opened and receive thread started. Communication can now begin." << endl);
00095 
00096   return true;
00097 }
00098 
00099 void SVHSerialInterface::close()
00100 {
00101   m_connected = false;
00102 
00103   // cancel and delete receive packet thread
00104   if (m_receive_thread)
00105   {
00106     // cancel thread
00107     m_receive_thread->cancel();
00108 
00109     m_receive_thread.reset();
00110 
00111     LOGGING_TRACE_C(DriverSVH, SVHSerialInterface, "Serial device receive thread was terminated." << endl);
00112   }
00113 
00114   // close and delete serial device handler
00115   if (m_serial_device)
00116   {
00117     m_serial_device->Close();
00118 
00119     m_serial_device.reset();
00120     LOGGING_TRACE_C(DriverSVH, SVHSerialInterface, "Serial device handle was closed and terminated." << endl);
00121   }
00122 }
00123 
00124 bool SVHSerialInterface::sendPacket(SVHSerialPacket& packet)
00125 {
00126   if (m_serial_device != NULL)
00127   {
00128     uint8_t check_sum1 = 0;
00129     uint8_t check_sum2 = 0;
00130 
00131     // Calculate Checksum for the packet
00132     for (size_t i = 0; i < packet.data.size(); i++)
00133     {
00134       check_sum1 += packet.data[i];
00135       check_sum2 ^= packet.data[i];
00136     }
00137 
00138     // set packet counter
00139     packet.index = static_cast<uint8_t>(m_packets_transmitted % uint8_t(-1));
00140 
00141     if (m_serial_device->IsOpen())
00142     {
00143       // Prepare arraybuilder
00144       size_t size = packet.data.size() + cPACKET_APPENDIX_SIZE;
00145       icl_comm::ArrayBuilder send_array(size);
00146       // Write header and packet information and checksum
00147       send_array << PACKET_HEADER1 << PACKET_HEADER2 << packet << check_sum1 << check_sum2;
00148 
00149       // actual hardware call to send the packet
00150       size_t bytes_send = 0;
00151       while (bytes_send < size)
00152       {
00153         bytes_send += m_serial_device->Write(send_array.array.data() + bytes_send, size - bytes_send);
00154       }
00155 
00156       // Small delay -> THIS SHOULD NOT BE NECESSARY as the communication speed should be handable by the HW. However, it will die if this sleep is
00157       // not used and this may also depend on your computer speed -> This issue might stem also from the hardware and will hopefully be fixed soon.
00158       icl_core::os::usleep(8000);
00159 
00160     }
00161     else
00162     {
00163       LOGGING_TRACE_C(DriverSVH, SVHSerialInterface, "sendPacket failed, serial device was not properly initialized." << endl);
00164       return false;
00165     }
00166 
00167     m_packets_transmitted++;
00168   }
00169 
00170   return true;
00171 }
00172 
00173 void SVHSerialInterface::resetTransmitPackageCount()
00174 {
00175   m_packets_transmitted = 0;
00176   // Only the receive thread knows abotu the accurate number it has received
00177   m_receive_thread->resetReceivedPackageCount();
00178 }
00179 
00180 void SVHSerialInterface::printPacketOnConsole(SVHSerialPacket &packet)
00181 {
00182 
00183   uint8_t check_sum1 = 0;
00184   uint8_t check_sum2 = 0;
00185 
00186   // Calculate Checksum for the packet
00187   for (size_t i = 0; i < packet.data.size(); i++)
00188   {
00189     check_sum1 += packet.data[i];
00190     check_sum2 ^= packet.data[i];
00191   }
00192 
00193   // set packet counter
00194   packet.index = static_cast<uint8_t>(m_dummy_packets_printed % uint8_t(-1));
00195 
00196 
00197   // Prepare arraybuilder
00198   size_t size = packet.data.size() + cPACKET_APPENDIX_SIZE;
00199   icl_comm::ArrayBuilder send_array(size);
00200   // Write header and packet information and checksum
00201   send_array << PACKET_HEADER1 << PACKET_HEADER2 << packet << check_sum1 << check_sum2;
00202 
00203   std::cout << send_array << std::endl;
00204 
00205   m_dummy_packets_printed++;
00206 }
00207 
00208 }


schunk_svh_driver
Author(s): Georg Heppner
autogenerated on Fri Apr 28 2017 02:31:08