srbridge.cpp
Go to the documentation of this file.
00001 
00024 #include <sr_edc_ethercat_drivers/srbridge.h>
00025 
00026 #include <iomanip>
00027 #include <sstream>
00028 
00029 #include <dll/ethercat_dll.h>
00030 #include <al/ethercat_AL.h>
00031 #include <dll/ethercat_device_addressed_telegram.h>
00032 #include <dll/ethercat_frame.h>
00033 
00034 PLUGINLIB_EXPORT_CLASS(SRBridge, EthercatDevice);
00035 
00036 void SRBridge::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00037 {
00038   SR0X::construct(sh, start_address);
00039   assert(sh_->get_product_code() == PRODUCT_CODE);
00040 
00041   ROS_INFO("Shadow Bridge configure -  %d @ %d", sh_->get_product_code(), sh_->get_ring_position());
00042 
00043   EtherCAT_FMMU_Config *fmmu;
00044   fmmu = new EtherCAT_FMMU_Config(0);
00045   sh_->set_fmmu_config(fmmu);
00046 
00047   EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(0);
00048   sh_->set_pd_config(pd);
00049 
00050 }
00051 
00052 int SRBridge::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00053 {
00054   int result = SR0X::initialize(hw, allow_unprogrammed);
00055   if (result != 0)
00056     return result;
00057 
00058   assert(sh_->get_product_code() == PRODUCT_CODE);
00059 
00060   if (device_offset_ != 0)
00061   {
00062     ROS_FATAL("Device offset of SRBRIDGE should be 0 not %d", device_offset_);
00063     return -1;
00064   }
00065 
00066   EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00067   uint16_t data, new_data;
00068   int rv;
00069 
00070   rv = readData(&com, (EC_UINT)EC_Slave_RD[PDI_Conf_reg].ado, (void *)&data, (EC_UINT)2);
00071   ROS_INFO("bridge port type: %s\n", data&1?"MII":"EBUS");
00072 
00073   rv = readData(&com, 0x100, &data, 2);
00074   if (rv != 0) ROS_ERROR("can't read open status");
00075 
00076   new_data = data & ~0xc000;
00077 
00078   rv = writeData(&com, 0x100, &new_data, 2);
00079   if (rv != 0) ROS_ERROR("can't write DL values");
00080 
00081   rv = readData(&com, 0x100, &data, 2);
00082   if (rv != 0) ROS_ERROR("can't read open status");
00083 
00084   return 0;
00085 }
00086 
00087 void SRBridge::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00088 {
00089   std::ostringstream str;
00090   str << "SRBridge : " << std::setw(2) << std::setfill('0') << sh_->get_ring_position();
00091   d.name = str.str();
00092   str.str("");
00093   str << sh_->get_product_code() << '-' << sh_->get_serial();
00094   d.hardware_id = str.str();
00095 
00096   d.message = "";
00097   d.level = 0;
00098 
00099   d.clear();
00100   d.addf("Position", "%02d", sh_->get_ring_position());
00101   d.addf("Product code", "%08x", sh_->get_product_code());
00102   d.addf("Serial", "%08x", sh_->get_serial());
00103   d.addf("Revision", "%08x", sh_->get_revision());
00104 
00105   this->ethercatDiagnostics(d, 2);
00106 }


sr_edc_ethercat_drivers
Author(s): Yann Sionneau, Ugo Cupcic / ugo@shadowrobot.com, software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:38:25