ethercat_device.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 "ethercat_hardware/ethercat_device.h"

#include <tinyxml.h>

#include <dll/ethercat_device_addressed_telegram.h>
#include <dll/ethercat_logical_addressed_telegram.h>
#include <dll/ethercat_frame.h>

#include <iomanip>

bool et1x00_error_counters::isGreaterThan(unsigned value) const
{
  if ((pdi_error>value) || (epu_error>value)) {
    return true;
  }

  for (unsigned i=0; i<4; ++i) {
    if ( (port[i].rx_error      > value) ||
         (forwarded_rx_error[i] > value) ||
         (lost_link[i]          > value) ||
         (port[i].invalid_frame > value) )
    {
      return true;
    }
  }
  return false;
}

bool et1x00_error_counters::isGreaterThan(const et1x00_error_counters &v) const
{
  if ((pdi_error>v.pdi_error) || (epu_error>v.epu_error)) {
    return true;
  }

  for (unsigned i=0; i<4; ++i) {
    if ( (port[i].rx_error      > v.port[i].rx_error     ) ||
         (forwarded_rx_error[i] > v.forwarded_rx_error[i]) ||
         (lost_link[i]          > v.lost_link[i]         ) ||
         (port[i].invalid_frame > v.port[i].invalid_frame) )
    {
      return true;
    }
  }
  return false;
}

bool et1x00_dl_status::hasLink(unsigned port)
{
  assert(port<4);
  return status & (1<<(4+port));
}

bool et1x00_dl_status::hasCommunication(unsigned port)
{
  assert(port<4);
  return status & (1<<(9+port*2));
}

bool et1x00_dl_status::isClosed(unsigned port)
{
  assert(port<4);
  return status & (1<<(8+port*2));
}

void et1x00_error_counters::zero()
{
  memset(this, 0, sizeof(et1x00_error_counters));
}


EthercatPortDiagnostics::EthercatPortDiagnostics() :
  hasLink(false),
  isClosed(false),
  hasCommunication(false)
{
  zeroTotals();
}

void EthercatPortDiagnostics::zeroTotals()
{
  rxErrorTotal=0;
  invalidFrameTotal=0;
  forwardedRxErrorTotal=0;
  lostLinkTotal=0;
}


EthercatDeviceDiagnostics::EthercatDeviceDiagnostics() :
  errorCountersMayBeCleared_(false),
  diagnosticsFirst_(true),
  diagnosticsValid_(false),
  resetDetected_(false),
  devicesRespondingToNodeAddress_(-1)
{
  zeroTotals();
  errorCountersPrev_.zero();
}

void EthercatDeviceDiagnostics::zeroTotals() 
{
  pdiErrorTotal_=0;
  epuErrorTotal_=0;
  portDiagnostics_[0].zeroTotals();
  portDiagnostics_[1].zeroTotals();
  portDiagnostics_[2].zeroTotals();
  portDiagnostics_[3].zeroTotals();  
}


void EthercatDeviceDiagnostics::accumulate(const et1x00_error_counters &n, const et1x00_error_counters &p)
{  
  pdiErrorTotal_ += n.pdi_error - p.pdi_error;
  epuErrorTotal_ += n.epu_error - p.epu_error; 
  for (unsigned i=0; i<4; ++i) {
    EthercatPortDiagnostics &pt(portDiagnostics_[i]);
    pt.rxErrorTotal           += n.port[i].rx_error      - p.port[i].rx_error     ;
    pt.forwardedRxErrorTotal  += n.forwarded_rx_error[i] - p.forwarded_rx_error[i];
    pt.lostLinkTotal          += n.lost_link[i]          - p.lost_link[i]         ;
    pt.invalidFrameTotal      += n.port[i].invalid_frame - p.port[i].invalid_frame;
  }
}


void EthercatDeviceDiagnostics::collect(EthercatCom *com, EtherCAT_SlaveHandler *sh)
{
  diagnosticsValid_ = false;
  diagnosticsFirst_ = false;

  // Check if device has been reset/power cycled using its node address
  // Node address initialize to 0 after device reset.
  // EML library will configure each node address to non-zero when it first starts
  // Device should respond to its node address, if it does not either:
  //  1. communication to device is not possible (lost/broken link)
  //  2. device was reset, and its fixed address setting is now 0
  { 
    // Send a packet with both a Fixed address read (NPRW) and a positional read (APRD)
    // If the NPRD has a working counter == 0, but the APRD sees the correct number of devices,
    // then the node has likely been reset.
    // Also, get DL status regiseter with nprd telegram
    EC_Logic *logic = EC_Logic::instance();
    et1x00_dl_status dl_status;
    NPRD_Telegram nprd_telegram(logic->get_idx(),
                                sh->get_station_address(),
                                dl_status.BASE_ADDR,
                                logic->get_wkc(),
                                sizeof(dl_status),
                                (unsigned char*) &dl_status);
    // Use positional read to re-count number of devices on chain
    unsigned char buf[1];    
    EC_UINT address = 0x0000;
    APRD_Telegram aprd_telegram(logic->get_idx(),  // Index
                                0,                 // Slave position on ethercat chain (auto increment address) (
                                address,           // ESC physical memory address (start address) 
                                logic->get_wkc(),  // Working counter
                                sizeof(buf),       // Data Length,
                                buf);              // Buffer to put read result into
    


    // Chain both telegrams together
    nprd_telegram.attach(&aprd_telegram);

    EC_Ethernet_Frame frame(&nprd_telegram);
    
    // Send/Recv data from slave
    if (!com->txandrx_once(&frame)) {
      // no response - broken link to device
      goto end;
    }

    devicesRespondingToNodeAddress_ = nprd_telegram.get_wkc();
    if (devicesRespondingToNodeAddress_ == 0) {
      // Device has not responded to its node address.
      if (aprd_telegram.get_adp() >= EtherCAT_AL::instance()->get_num_slaves()) {
        resetDetected_ = true;
        goto end;
      }
    } 
    else if (devicesRespondingToNodeAddress_ > 1) {
      // Can't determine much if two (or more) devices are responding to same request.
      goto end;
    }
    else {
      resetDetected_ = false;
    }
    
    // fill in port status information
    for (unsigned i=0;i<4;++i) {
      EthercatPortDiagnostics &pt(portDiagnostics_[i]);
      pt.hasLink  = dl_status.hasLink(i);
      pt.isClosed = dl_status.isClosed(i);
      pt.hasCommunication = dl_status.hasCommunication(i);
    }
  }

  { // read and accumulate communication error counters
    et1x00_error_counters e;
    assert(sizeof(e) == (0x314-0x300));
    if (0 != EthercatDevice::readData(com, sh, e.BASE_ADDR, &e, sizeof(e), EthercatDevice::FIXED_ADDR)) {
      goto end;
    }   

    // If this previously tried to clear the error counters but d/n get a response
    // then use the newly read values to guess if they got cleared or not.
    if (errorCountersMayBeCleared_) {
      if (!e.isGreaterThan(errorCountersPrev_)) {
        errorCountersPrev_.zero();
      }
      errorCountersMayBeCleared_ = false;
    }
    if (errorCountersPrev_.isGreaterThan(e)) {
      ROS_ERROR("Device %d : previous port error counters less current values", sh->get_ring_position());
    }

    // Accumulate
    this->accumulate(e, errorCountersPrev_);
    errorCountersPrev_ = e;

    // re-read and clear communication error counters    
    if(e.isGreaterThan(50)) {
      if (0 != EthercatDevice::readWriteData(com, sh, e.BASE_ADDR, &e, sizeof(e), EthercatDevice::FIXED_ADDR)) {
        // Packet got lost... Can't know for sure that error counters got cleared
        errorCountersMayBeCleared_ = true;
        goto end;
      }
      // We read and cleared error counters in same packet, accumulate any error counter differences
      this->accumulate(e, errorCountersPrev_);
      errorCountersPrev_.zero();
    }
  }
  
  // Everything was read successfully
  diagnosticsValid_ = true;
  
 end:
  return;
}

void EthercatDeviceDiagnostics::publish(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts) const
{
  if (numPorts>4) {
    assert(numPorts<4);
    numPorts=4;
  }
  assert(numPorts > 0);
  
  d.addf("Reset detected", "%s", (resetDetected_ ? "Yes" : "No"));
  d.addf("Valid", "%s", (diagnosticsValid_ ? "Yes" : "No"));

  d.addf("EPU Errors", "%lld", epuErrorTotal_);
  d.addf("PDI Errors", "%lld", pdiErrorTotal_);
  ostringstream os, port;
  for (unsigned i=0; i<numPorts; ++i) {
    const EthercatPortDiagnostics &pt(portDiagnostics_[i]);
    port.str(""); port << " Port " << i;
    os.str(""); os << "Status" << port.str();
    d.addf(os.str(), "%s Link, %s, %s Comm", 
           pt.hasLink ? "Has":"No",
           pt.isClosed ? "Closed":"Open",
           pt.hasCommunication ? "Has":"No"); 
    os.str(""); os << "RX Error" << port.str();
    d.addf(os.str(), "%lld", pt.rxErrorTotal); 
    os.str(""); os << "Forwarded RX Error" << port.str();
    d.addf(os.str(), "%lld", pt.forwardedRxErrorTotal); 
    os.str(""); os << "Invalid Frame" << port.str();
    d.addf(os.str(), "%lld", pt.invalidFrameTotal);
    os.str(""); os << "Lost Link" << port.str();
    d.addf(os.str(), "%lld", pt.lostLinkTotal);
  }
  
  if (resetDetected_) 
  {
    d.mergeSummaryf(d.ERROR, "Device reset likely");
  }
  else if (devicesRespondingToNodeAddress_ > 1) 
  {
    d.mergeSummaryf(d.ERROR, "More than one device (%d) responded to node address", devicesRespondingToNodeAddress_);
  }
  else {
    if (diagnosticsFirst_)
    {
      d.mergeSummaryf(d.WARN, "Have not yet collected diagnostics");
    }
    else if (!diagnosticsValid_) 
    {
      d.mergeSummaryf(d.WARN, "Could not collect diagnostics");
    }
    else 
    {
      if (!portDiagnostics_[0].hasLink) {
        d.mergeSummaryf(d.WARN, "No link on port 0");
      }
    }
  }
}

void EthercatDevice::construct(EtherCAT_SlaveHandler *sh, int &start_address)
{
  sh_ = sh;
}

void EthercatDevice::construct(ros::NodeHandle &nh)
{
  // empty
}


EthercatDevice::EthercatDevice() : use_ros_(true)
{
  sh_ = NULL;
  command_size_ = 0;
  status_size_ = 0;
  newDiagnosticsIndex_ = 0;

  int error = pthread_mutex_init(&newDiagnosticsIndexLock_, NULL);
  if (error != 0) {
    ROS_FATAL("Initializing indexLock failed : %s", strerror(error));
    sleep(1); // wait for ros to flush rosconsole output
    exit(EXIT_FAILURE);
  }

  error = pthread_mutex_init(&diagnosticsLock_, NULL);
  if (error != 0) {
    ROS_FATAL("Initializing diagnositcsLock failed : %s", strerror(error));
    sleep(1); // wait for ros to flush rosconsole output
    exit(EXIT_FAILURE);
  }    
}

EthercatDevice::~EthercatDevice()
{
  //nothing
}

void EthercatDevice::collectDiagnostics(EthercatCom *com)
{
  // Really, should not need this lock, since there should only be one thread updating diagnostics.
  pthread_mutex_lock(&diagnosticsLock_);  
  
  // Get references to diagnostics... code is easier to read
  unsigned oldDiagnosticsIndex = (newDiagnosticsIndex_+1)&1;
  const EthercatDeviceDiagnostics &newDiag = deviceDiagnostics[newDiagnosticsIndex_];
  EthercatDeviceDiagnostics &oldDiag = deviceDiagnostics[oldDiagnosticsIndex];

  // copy new diagnostics values in old diagnostics, because diagnostic data use accumumlators
  oldDiag = newDiag; 

  // Collect diagnostics data into "old" buffer.  
  // This way the "new" buffer is never changed while the publishing thread may be using it.
  oldDiag.collect(com, sh_);

  // Got new diagnostics... swap buffers.  
  // Publisher thread uses "new" buffer.  New to lock while swapping buffers.  
  // Note : this is just changing an integer value, so it only takes a couple of instructions.  
  pthread_mutex_lock(&newDiagnosticsIndexLock_);    
  newDiagnosticsIndex_=oldDiagnosticsIndex;
  pthread_mutex_unlock(&newDiagnosticsIndexLock_);
  
  // Done, unlock
  pthread_mutex_unlock(&diagnosticsLock_);
}


int EthercatDevice::readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh,  EC_UINT address, void* buffer, EC_UINT length, AddrMode addrMode)
{
  unsigned char *p = (unsigned char *)buffer;
  EC_Logic *logic = EC_Logic::instance();
  
  NPRW_Telegram nprw_telegram(logic->get_idx(),
                              sh->get_station_address(),
                              address,
                              logic->get_wkc(),
                              length,
                              p);
  
  APRW_Telegram aprw_telegram(logic->get_idx(),  // Index
                              -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
                              address,               // ESC physical memory address (start address) 
                              logic->get_wkc(),  // Working counter
                              length,                   // Data Length,
                              p);             // Buffer to put read result into

  // Put read telegram in ethercat/ethernet frame  
  EC_Telegram * telegram = NULL;
  if (addrMode == FIXED_ADDR) {
    telegram = &nprw_telegram;
  } else if (addrMode == POSITIONAL_ADDR) {
    telegram = &aprw_telegram;
  } else {
    assert(0);
    return -1;
  }
  
  // Put telegram in ethercat/ethernet frame
  EC_Ethernet_Frame frame(telegram);
  
  // Send/Recv data from slave
  if (!com->txandrx_once(&frame)) {
    return -1;
  }

  // In some cases (clearing status mailbox) this is expected to occur
  if (telegram->get_wkc() != 3) {
    return -2;
  }

  return 0;  
}


int EthercatDevice::readData(EthercatCom *com, EtherCAT_SlaveHandler *sh,  EC_UINT address, void* buffer, EC_UINT length, AddrMode addrMode)
{
  unsigned char *p = (unsigned char *)buffer;
  EC_Logic *logic = EC_Logic::instance();
  
  NPRD_Telegram nprd_telegram(logic->get_idx(),
                              sh->get_station_address(),
                              address,
                              logic->get_wkc(),
                              length,
                              p);
  
  APRD_Telegram aprd_telegram(logic->get_idx(),  // Index
                              -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
                              address,               // ESC physical memory address (start address) 
                              logic->get_wkc(),  // Working counter
                              length,                   // Data Length,
                              p);             // Buffer to put read result into

  // Put read telegram in ethercat/ethernet frame  
  EC_Telegram * telegram = NULL;
  if (addrMode == FIXED_ADDR) {
    telegram = &nprd_telegram;
  } else if (addrMode == POSITIONAL_ADDR) {
    telegram = &aprd_telegram;
  } else {
    assert(0);
    return -1;
  }

  // Put telegram in ethercat/ethernet frame
  EC_Ethernet_Frame frame(telegram);

  // Send/Recv data from slave
  if (!com->txandrx(&frame)) {
    return -1;
  }

  // In some cases (clearing status mailbox) this is expected to occur
  if (telegram->get_wkc() != 1) {
    return -2;
  }

  return 0;
}


int EthercatDevice::writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh,  EC_UINT address, void const* buffer, EC_UINT length, AddrMode addrMode)
{
  unsigned char const *p = (unsigned char const*)buffer;
  EC_Logic *logic = EC_Logic::instance();

  NPWR_Telegram npwr_telegram(logic->get_idx(),
                              sh->get_station_address(),
                              address,
                              logic->get_wkc(),
                              length,
                              p);
  
  APWR_Telegram apwr_telegram(logic->get_idx(),  // Index
                              -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
                              address,               // ESC physical memory address (start address) 
                              logic->get_wkc(),  // Working counter
                              length,                   // Data Length,
                              p);             // Buffer to put read result into

  // Put read telegram in ethercat/ethernet frame  
  EC_Telegram * telegram = NULL;
  if (addrMode == FIXED_ADDR) {
    telegram = &npwr_telegram;
  } else if (addrMode == POSITIONAL_ADDR) {
    telegram = &apwr_telegram;
  } else {
    assert(0);
    return -1;
  }

  // Put telegram in ethercat/ethernet frame
  EC_Ethernet_Frame frame(telegram);

  // Send/Recv data from slave
  if (!com->txandrx(&frame)) {
    return -1;
  }
  
  if (telegram->get_wkc() != 1) {
    return -2;
  }
  
  return 0;
}


void EthercatDevice::ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts)
{
  if (numPorts>4) {
    assert(numPorts<4);
    numPorts=4;
  }

  // By locking index, diagnostics double-buffer cannot be swapped.
  // Update thread is only allowed to change 'old' diagnostics buffer, 
  //  so new buffer data cannot be changed while index lock is held.
  pthread_mutex_lock(&newDiagnosticsIndexLock_);

  const EthercatDeviceDiagnostics &newDiag = deviceDiagnostics[newDiagnosticsIndex_];
  newDiag.publish(d, numPorts);

  pthread_mutex_unlock(&newDiagnosticsIndexLock_);
}


void EthercatDevice::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
{
  stringstream str;
  str << "EtherCAT Device (" << std::setw(2) << std::setfill('0') << sh_->get_ring_position() << ")";
  d.name = str.str();
  str.str("");
  str << sh_->get_product_code() << '-' << sh_->get_serial();
  d.hardware_id = str.str();

  d.message = "";
  d.level = 0;

  d.clear();
  d.addf("Position", "%02d", sh_->get_ring_position());
  d.addf("Product code", "%08x", sh_->get_product_code());
  d.addf("Serial", "%08x", sh_->get_serial());
  d.addf("Revision", "%08x", sh_->get_revision());

  this->ethercatDiagnostics(d, 4); //assume unknown device has 4 ports
}

void EthercatDevice::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
{
  // Clean up recycled status object before reusing it.
  diagnostic_status_.clearSummary();
  diagnostic_status_.clear();

  // If child-class does not implement multiDiagnostics(), fall back to using slave's diagnostic() function
  diagnostics(diagnostic_status_, buffer);
  vec.push_back(diagnostic_status_);
}