SVHReceiveThread.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/SVHReceiveThread.h>
00031 #include <driver_svh/Logging.h>
00032 
00033 
00034 using icl_comm::ArrayBuilder;
00035 
00036 namespace driver_svh {
00037 
00038 SVHReceiveThread::SVHReceiveThread(const TimeSpan& period, boost::shared_ptr<Serial> device,
00039                                      ReceivedPacketCallback const & received_callback)
00040   : PeriodicThread("SVHReceiveThread", period),
00041     m_serial_device(device),
00042     m_received_state(eRS_HEADER1),
00043     m_length(0),
00044     m_data(0, 0),
00045     m_ab(0),
00046     m_packets_received(0),
00047     m_received_callback(received_callback)
00048 {}
00049 
00050 void SVHReceiveThread::run()
00051 {
00052   while (execute())
00053   {
00054     if (m_serial_device)// != NULL)
00055     {
00056       if (m_serial_device->IsOpen())
00057       {
00058         // All we every want to do is receiving data :)
00059         receiveData();
00060       }
00061       else
00062       {
00063         LOGGING_WARNING_C(DriverSVH, SVHReceiveThread, "Cannot read data from serial device. It is not opened!" << endl);
00064       }
00065     }
00066 
00067     // Wait for the thread period so that the timing is in sync.
00068     waitPeriod();
00069   }
00070 }
00071 
00072 bool SVHReceiveThread::receiveData()
00073 {
00074 
00075   /*
00076    * Each packet has to follow the defined packet structure which is ensured by the following state machine.
00077    * The "Bytestream" (not realy a stream) is interpreted byte by byte. If the structure is still right the
00078    * next state is entered, if a wrong byte is detected the whole packet is discarded and the SM switches to
00079    * the synchronization state aggain.
00080    * If the SM reaches the final state the packet will be given to the packet handler to decide what to do with
00081    * its content.
00082    *  NOTE: All layers working with a SerialPacket (except this one) assume that the packet has a valid structure
00083    *        and all data fields present.
00084    */
00085 
00086   switch (m_received_state)
00087   {
00088     case eRS_HEADER1:
00089     {
00090       uint8_t data_byte = 0;
00091       if (m_serial_device->Read(&data_byte, sizeof(uint8_t)))
00092       {
00093         if (data_byte == PACKET_HEADER1)
00094         {
00095           m_received_state = eRS_HEADER2;
00096         }
00097       }
00098       break;
00099     }
00100     case eRS_HEADER2:
00101     {
00102       uint8_t data_byte = 0;
00103       if (m_serial_device->Read(&data_byte, sizeof(uint8_t)))
00104       {
00105         switch (data_byte)
00106         {
00107           case PACKET_HEADER2:
00108           {
00109             m_received_state = eRS_INDEX;
00110             break;
00111           }
00112           case PACKET_HEADER1:
00113           {
00114             m_received_state = eRS_HEADER2;
00115             break;
00116           }
00117           default:
00118           {
00119             m_received_state = eRS_HEADER1;
00120             break;
00121           }
00122         }
00123       }
00124       break;
00125     }
00126     case eRS_INDEX:
00127     {
00128       // Reset Array Builder for each fresh packet
00129       m_ab.reset(0);
00130 
00131       uint8_t index = 0;
00132       if (m_serial_device->Read(&index, sizeof(uint8_t)))
00133       {
00134         // Data bytes are not cenverted in endianess at this point
00135         m_ab.appendWithoutConversion(index);
00136         m_received_state = eRS_ADDRESS;
00137       }
00138       break;
00139     }
00140     case eRS_ADDRESS:
00141     {
00142       //get the address
00143       uint8_t address = 0;
00144       if (m_serial_device->Read(&address, sizeof(uint8_t)))
00145       {
00146         m_ab.appendWithoutConversion(address);
00147         m_received_state = eRS_LENGTH;
00148       }
00149       break;
00150     }
00151     case eRS_LENGTH:
00152     {
00153       // get payload length
00154       uint16_t length = 0;
00155       if (m_serial_device->Read(&length, sizeof(uint16_t)))
00156       {
00157         m_ab.appendWithoutConversion(length);
00158         m_length = m_ab.readBack<uint16_t>();
00159         m_received_state = eRS_DATA;
00160       }
00161       break;
00162     }
00163     case eRS_DATA:
00164     {
00165       // get the payload itself
00166       // Some conversion due to legacy hardware calls
00167       uint8_t buffer[m_length];
00168       if (m_serial_device->Read(reinterpret_cast<void *>(buffer), m_length))
00169       {
00170         m_data.clear();
00171         m_data.insert(m_data.end(), &buffer[0], &buffer[m_length]);
00172 
00173         m_ab.appendWithoutConversion(m_data);
00174         m_received_state = eRS_CHECKSUM;
00175       }
00176       break;
00177     }
00178     case eRS_CHECKSUM:
00179     {
00180       uint8_t checksum1 = 0;
00181       uint8_t checksum2 = 0;
00182       if (m_serial_device->Read(&checksum1, sizeof(uint8_t))
00183           && m_serial_device->Read(&checksum2, sizeof(uint8_t)))
00184       {
00185         // probe for correct checksum
00186         for (size_t i = 0; i < m_data.size(); ++i)
00187         {
00188           checksum1 -= m_data[i];
00189           checksum2 ^= m_data[i];
00190         }
00191 
00192         if ((checksum1 == 0) && (checksum2 == 0))
00193         {
00194           m_received_state = eRS_COMPLETE;
00195         }
00196         else
00197         {
00198           m_received_state = eRS_HEADER1;
00199         }
00200       }
00201       else
00202       {
00203         LOGGING_TRACE_C(DriverSVH, SVHReceiveThread, "Could not read checksum bytes from serial port" << endl);
00204       }
00205       break;
00206     }
00207     case eRS_COMPLETE:
00208     {
00209       // start with an empty package
00210       // Warning: It is imperative for correct readouts to create the received_packet with the correct length!
00211       SVHSerialPacket received_packet(m_length);
00212       m_ab >> received_packet;
00213 
00214       m_packets_received++;
00215 
00216       // notify whoever is waiting for this
00217       if (m_received_callback)
00218       {
00219         m_received_callback(received_packet, m_packets_received);
00220       }
00221 
00222       m_received_state = eRS_HEADER1;
00223       break;
00224     }
00225   }
00226 
00227   return true;
00228 }
00229 
00230 }


schunk_svh_driver
Author(s): Georg Heppner
autogenerated on Fri Aug 28 2015 12:59:19