srbridge.cpp
Go to the documentation of this file.
1 
24 
25 #include <iomanip>
26 #include <sstream>
27 #include <cassert>
28 
29 PLUGINLIB_EXPORT_CLASS(SRBridge, EthercatDevice);
30 
31 void SRBridge::construct(EtherCAT_SlaveHandler *sh, int &start_address)
32 {
33  EthercatDevice::construct(sh, start_address);
34 
35  assert(sh_->get_product_code() == PRODUCT_CODE);
36 
37  ROS_INFO("Shadow Bridge configure - %d @ %d", sh_->get_product_code(), sh_->get_ring_position());
38 }
39 
40 int SRBridge::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
41 {
42  SR0X::initialize(hw, allow_unprogrammed);
43 
44  assert(sh_->get_product_code() == PRODUCT_CODE);
45 
46  EthercatDirectCom com(sh_->m_router_instance->m_al_instance->m_dll_instance);
47  uint16_t data, new_data;
48  int rv;
49 
50  rv = readData(&com, (uint16_t) EC_Slave_RD[PDI_Conf_reg].ado, &data, 2, FIXED_ADDR);
51  ROS_INFO("bridge port type: %s", data & 1 ? "MII" : "EBUS");
52 
53  rv = readData(&com, 0x100, &data, 2, FIXED_ADDR);
54  if (rv != 0)
55  ROS_ERROR("can't read open status");
56 
57  new_data = data & ~0xc000;
58 
59  rv = writeData(&com, 0x100, &new_data, 2, FIXED_ADDR);
60  if (rv != 0)
61  ROS_ERROR("can't write DL values");
62 
63  rv = readData(&com, 0x100, &data, 2, FIXED_ADDR);
64  if (rv != 0)
65  ROS_ERROR("can't read open status");
66 
67  return 0;
68 }
69 
71 {
72  std::ostringstream str;
73  str << "SRBridge : " << std::setw(2) << std::setfill('0') << sh_->get_ring_position();
74  d.name = str.str();
75  str.str("");
76  str << sh_->get_product_code() << '-' << sh_->get_serial();
77  d.hardware_id = str.str();
78 
79  d.message = "";
80  d.level = 0;
81 
82  d.clear();
83  d.addf("Position", "%02d", sh_->get_ring_position());
84  d.addf("Product code", "%08x", sh_->get_product_code());
85  d.addf("Serial", "%08x", sh_->get_serial());
86  d.addf("Revision", "%08x", sh_->get_revision());
87 
88  this->ethercatDiagnostics(d, 2);
89 }
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)
Definition: sr0x.cpp:36
virtual void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
Definition: srbridge.cpp:70
data
void addf(const std::string &key, const char *format,...)
#define ROS_INFO(...)
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
Definition: srbridge.cpp:40
This is a ROS driver for the etherCAT bridge.
virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address)
Definition: srbridge.cpp:31
PLUGINLIB_EXPORT_CLASS(SRBridge, EthercatDevice)
#define ROS_ERROR(...)


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:53