ethercat_com.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "ros_ethercat_hardware/ethercat_com.h"
00036 #include <stdio.h>
00037 #include <errno.h>
00038 #include <assert.h>
00039 
00040 bool EthercatDirectCom::txandrx_once(struct EtherCAT_Frame * frame)
00041 {
00042   assert(frame != NULL);
00043   int handle = dll_->tx(frame);
00044   if (handle < 0)
00045     return false;
00046   return dll_->rx(frame, handle);
00047 }
00048 
00049 bool EthercatDirectCom::txandrx(struct EtherCAT_Frame * frame)
00050 {
00051   return dll_->txandrx(frame);
00052 }
00053 
00054 EthercatOobCom::EthercatOobCom(struct netif *ni) :
00055   ni_(ni),
00056   state_(IDLE),
00057   frame_(NULL),
00058   handle_(-1),
00059   line_(0)
00060 {
00061   assert(ni_ != NULL);
00062 
00063   pthread_mutexattr_t mutex_attr;
00064   int error = pthread_mutexattr_init(&mutex_attr);
00065   if (error != 0)
00066   {
00067     fprintf(stderr, "%s : Initializing mutex attr failed : %d\n", __func__, error);
00068     return;
00069   }
00070   error = pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
00071   if (error != 0)
00072   {
00073     fprintf(stderr, "%s : Setting type of mutex attr failed : %d\n", __func__, error);
00074     return;
00075   }
00076   error = pthread_mutex_init(&mutex_, &mutex_attr);
00077   if (error != 0)
00078   {
00079     fprintf(stderr, "%s : Initializing mutex failed : %d\n", __func__, error);
00080     return;
00081   }
00082   error = pthread_cond_init(&share_cond_, NULL);
00083   if (error != 0)
00084   {
00085     fprintf(stderr, "%s : Initializing share condition failed : %d\n", __func__, error);
00086     return;
00087   }
00088   error = pthread_cond_init(&busy_cond_, NULL);
00089   if (error != 0)
00090     fprintf(stderr, "%s : Initializing busy condition failed : %d\n", __func__, error);
00091   return;
00092 }
00093 
00094 bool EthercatOobCom::lock(unsigned line)
00095 {
00096   int error;
00097   if (0 != (error = pthread_mutex_lock(&mutex_)))
00098   {
00099     fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
00100     return false;
00101   }
00102   line_ = line;
00103   return true;
00104 }
00105 
00106 bool EthercatOobCom::trylock(unsigned line)
00107 {
00108   int error;
00109   if (0 != (error = pthread_mutex_trylock(&mutex_)))
00110   {
00111     if (error != EBUSY)
00112       fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
00113     return false;
00114   }
00115   line_ = line;
00116   return true;
00117 }
00118 
00119 bool EthercatOobCom::unlock(unsigned line)
00120 {
00121   int error;
00122   if (0 != (error = pthread_mutex_unlock(&mutex_)))
00123   {
00124     fprintf(stderr, "%s : unlock %d at %d\n", __func__, error, line);
00125     return false;
00126   }
00127   line_ = 0;
00128   return true;
00129 }
00130 
00131 // OOB replacement for netif->txandrx()
00132 // Returns true for success, false for dropped packet
00133 
00134 bool EthercatOobCom::txandrx_once(struct EtherCAT_Frame * frame)
00135 {
00136   assert(frame != NULL);
00137 
00138   if (!lock(__LINE__))
00139     return false;
00140 
00141   // Wait for an opening to send frame
00142   while (state_ != IDLE)
00143   {
00144     pthread_cond_wait(&share_cond_, &mutex_);
00145   }
00146   frame_ = frame;
00147   state_ = READY_TO_SEND;
00148 
00149   // RT control loop will send frame
00150   do
00151   {
00152     pthread_cond_wait(&busy_cond_, &mutex_);
00153   }
00154   while (state_ != WAITING_TO_RECV);
00155 
00156   // Packet has been sent, wait for recv
00157   bool success = false;
00158   if (handle_ >= 0)
00159     success = ni_->rx(frame_, ni_, handle_);
00160   handle_ = -1;
00161 
00162   // Allow other threads to send data
00163   assert(frame_ == frame);
00164   state_ = IDLE;
00165   pthread_cond_signal(&share_cond_);
00166 
00167   unlock(__LINE__);
00168 
00169   return success;
00170 }
00171 
00172 bool EthercatOobCom::txandrx(struct EtherCAT_Frame * frame)
00173 {
00174   static const unsigned MAX_TRIES = 10;
00175   for (unsigned tries = 0; tries < MAX_TRIES; ++tries)
00176   {
00177     if (this->txandrx_once(frame))
00178       return true;
00179   }
00180   return false;
00181 }
00182 
00183 // Called by RT control loop to send oob data
00184 
00185 void EthercatOobCom::tx()
00186 {
00187   if (!trylock(__LINE__))
00188     return;
00189 
00190   if (state_ == READY_TO_SEND)
00191   {
00192     // Packet is in need of being sent
00193     assert(frame_ != NULL);
00194     handle_ = ni_->tx(frame_, ni_);
00195     state_ = WAITING_TO_RECV;
00196     pthread_cond_signal(&busy_cond_);
00197   }
00198 
00199   unlock(__LINE__);
00200 }


ros_ethercat_hardware
Author(s): Rob Wheeler , Derek King , Manos Nikolaidis
autogenerated on Thu Jul 4 2019 20:01:53