ethercat_com.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include "ros_ethercat_hardware/ethercat_com.h"
#include <stdio.h>
#include <errno.h>
#include <assert.h>

bool EthercatDirectCom::txandrx_once(struct EtherCAT_Frame * frame)
{
  assert(frame != NULL);
  int handle = dll_->tx(frame);
  if (handle < 0)
    return false;
  return dll_->rx(frame, handle);
}

bool EthercatDirectCom::txandrx(struct EtherCAT_Frame * frame)
{
  return dll_->txandrx(frame);
}

EthercatOobCom::EthercatOobCom(struct netif *ni) :
  ni_(ni),
  state_(IDLE),
  frame_(NULL),
  handle_(-1),
  line_(0)
{
  assert(ni_ != NULL);

  pthread_mutexattr_t mutex_attr;
  int error = pthread_mutexattr_init(&mutex_attr);
  if (error != 0)
  {
    fprintf(stderr, "%s : Initializing mutex attr failed : %d\n", __func__, error);
    return;
  }
  error = pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
  if (error != 0)
  {
    fprintf(stderr, "%s : Setting type of mutex attr failed : %d\n", __func__, error);
    return;
  }
  error = pthread_mutex_init(&mutex_, &mutex_attr);
  if (error != 0)
  {
    fprintf(stderr, "%s : Initializing mutex failed : %d\n", __func__, error);
    return;
  }
  error = pthread_cond_init(&share_cond_, NULL);
  if (error != 0)
  {
    fprintf(stderr, "%s : Initializing share condition failed : %d\n", __func__, error);
    return;
  }
  error = pthread_cond_init(&busy_cond_, NULL);
  if (error != 0)
    fprintf(stderr, "%s : Initializing busy condition failed : %d\n", __func__, error);
  return;
}

bool EthercatOobCom::lock(unsigned line)
{
  int error;
  if (0 != (error = pthread_mutex_lock(&mutex_)))
  {
    fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
    return false;
  }
  line_ = line;
  return true;
}

bool EthercatOobCom::trylock(unsigned line)
{
  int error;
  if (0 != (error = pthread_mutex_trylock(&mutex_)))
  {
    if (error != EBUSY)
      fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
    return false;
  }
  line_ = line;
  return true;
}

bool EthercatOobCom::unlock(unsigned line)
{
  int error;
  if (0 != (error = pthread_mutex_unlock(&mutex_)))
  {
    fprintf(stderr, "%s : unlock %d at %d\n", __func__, error, line);
    return false;
  }
  line_ = 0;
  return true;
}

// OOB replacement for netif->txandrx()
// Returns true for success, false for dropped packet

bool EthercatOobCom::txandrx_once(struct EtherCAT_Frame * frame)
{
  assert(frame != NULL);

  if (!lock(__LINE__))
    return false;

  // Wait for an opening to send frame
  while (state_ != IDLE)
  {
    pthread_cond_wait(&share_cond_, &mutex_);
  }
  frame_ = frame;
  state_ = READY_TO_SEND;

  // RT control loop will send frame
  do
  {
    pthread_cond_wait(&busy_cond_, &mutex_);
  }
  while (state_ != WAITING_TO_RECV);

  // Packet has been sent, wait for recv
  bool success = false;
  if (handle_ >= 0)
    success = ni_->rx(frame_, ni_, handle_);
  handle_ = -1;

  // Allow other threads to send data
  assert(frame_ == frame);
  state_ = IDLE;
  pthread_cond_signal(&share_cond_);

  unlock(__LINE__);

  return success;
}

bool EthercatOobCom::txandrx(struct EtherCAT_Frame * frame)
{
  static const unsigned MAX_TRIES = 10;
  for (unsigned tries = 0; tries < MAX_TRIES; ++tries)
  {
    if (this->txandrx_once(frame))
      return true;
  }
  return false;
}

// Called by RT control loop to send oob data

void EthercatOobCom::tx()
{
  if (!trylock(__LINE__))
    return;

  if (state_ == READY_TO_SEND)
  {
    // Packet is in need of being sent
    assert(frame_ != NULL);
    handle_ = ni_->tx(frame_, ni_);
    state_ = WAITING_TO_RECV;
    pthread_cond_signal(&busy_cond_);
  }

  unlock(__LINE__);
}