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 }