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 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00061 uint16_t data, new_data;
00062 int rv;
00063
00064 rv = readData(&com, (EC_UINT)EC_Slave_RD[PDI_Conf_reg].ado, (void *)&data, (EC_UINT)2);
00065 ROS_INFO("bridge port type: %s\n", data&1?"MII":"EBUS");
00066
00067 rv = readData(&com, 0x100, &data, 2);
00068 if (rv != 0) ROS_ERROR("can't read open status");
00069
00070 new_data = data & ~0xc000;
00071
00072 rv = writeData(&com, 0x100, &new_data, 2);
00073 if (rv != 0) ROS_ERROR("can't write DL values");
00074
00075 rv = readData(&com, 0x100, &data, 2);
00076 if (rv != 0) ROS_ERROR("can't read open status");
00077
00078 return 0;
00079 }
00080
00081 void SRBridge::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00082 {
00083 std::ostringstream str;
00084 str << "SRBridge : " << std::setw(2) << std::setfill('0') << sh_->get_ring_position();
00085 d.name = str.str();
00086 str.str("");
00087 str << sh_->get_product_code() << '-' << sh_->get_serial();
00088 d.hardware_id = str.str();
00089
00090 d.message = "";
00091 d.level = 0;
00092
00093 d.clear();
00094 d.addf("Position", "%02d", sh_->get_ring_position());
00095 d.addf("Product code", "%08x", sh_->get_product_code());
00096 d.addf("Serial", "%08x", sh_->get_serial());
00097 d.addf("Revision", "%08x", sh_->get_revision());
00098
00099 this->ethercatDiagnostics(d, 2);
00100 }