Go to the documentation of this file.00001
00024 #include <sr_edc_ethercat_drivers/srbridge.h>
00025
00026 #include <iomanip>
00027 #include <sstream>
00028 #include <cassert>
00029
00030 PLUGINLIB_EXPORT_CLASS(SRBridge, EthercatDevice);
00031
00032 void SRBridge::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00033 {
00034 EthercatDevice::construct(sh, start_address);
00035
00036 assert(sh_->get_product_code() == PRODUCT_CODE);
00037
00038 ROS_INFO("Shadow Bridge configure - %d @ %d", sh_->get_product_code(), sh_->get_ring_position());
00039 }
00040
00041 int SRBridge::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00042 {
00043 SR0X::initialize(hw, allow_unprogrammed);
00044
00045 assert(sh_->get_product_code() == PRODUCT_CODE);
00046
00047 EthercatDirectCom com(sh_->m_router_instance->m_al_instance->m_dll_instance);
00048 uint16_t data, new_data;
00049 int rv;
00050
00051 rv = readData(&com, (uint16_t) EC_Slave_RD[PDI_Conf_reg].ado, &data, 2, FIXED_ADDR);
00052 ROS_INFO("bridge port type: %s", data & 1 ? "MII" : "EBUS");
00053
00054 rv = readData(&com, 0x100, &data, 2, FIXED_ADDR);
00055 if (rv != 0)
00056 ROS_ERROR("can't read open status");
00057
00058 new_data = data & ~0xc000;
00059
00060 rv = writeData(&com, 0x100, &new_data, 2, FIXED_ADDR);
00061 if (rv != 0)
00062 ROS_ERROR("can't write DL values");
00063
00064 rv = readData(&com, 0x100, &data, 2, FIXED_ADDR);
00065 if (rv != 0)
00066 ROS_ERROR("can't read open status");
00067
00068 return 0;
00069 }
00070
00071 void SRBridge::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00072 {
00073 std::ostringstream str;
00074 str << "SRBridge : " << std::setw(2) << std::setfill('0') << sh_->get_ring_position();
00075 d.name = str.str();
00076 str.str("");
00077 str << sh_->get_product_code() << '-' << sh_->get_serial();
00078 d.hardware_id = str.str();
00079
00080 d.message = "";
00081 d.level = 0;
00082
00083 d.clear();
00084 d.addf("Position", "%02d", sh_->get_ring_position());
00085 d.addf("Product code", "%08x", sh_->get_product_code());
00086 d.addf("Serial", "%08x", sh_->get_serial());
00087 d.addf("Revision", "%08x", sh_->get_revision());
00088
00089 this->ethercatDiagnostics(d, 2);
00090 }