Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <ethercat_hardware/wg014.h>
00036 #include <iomanip>
00037
00038 #include <ros/console.h>
00039
00040 PLUGINLIB_DECLARE_CLASS(ethercat_hardware, 6805014, WG014, EthercatDevice);
00041
00042 void WG014::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00043 {
00044 EthercatDevice::construct(sh, start_address);
00045 sh->set_fmmu_config( new EtherCAT_FMMU_Config(0) );
00046 sh->set_pd_config( new EtherCAT_PD_Config(0) );
00047
00048 fw_major_ = (sh->get_revision() >> 8) & 0xff;
00049 fw_minor_ = sh->get_revision() & 0xff;
00050 board_major_ = ((sh->get_revision() >> 24) & 0xff) - 1;
00051 board_minor_ = (sh->get_revision() >> 16) & 0xff;
00052 }
00053
00054 WG014::~WG014()
00055 {
00056 delete sh_->get_fmmu_config();
00057 delete sh_->get_pd_config();
00058 }
00059
00060 int WG014::initialize(pr2_hardware_interface::HardwareInterface *, bool)
00061 {
00062 ROS_DEBUG("Device #%02d: WG014 (%#08x)", sh_->get_ring_position(), sh_->get_product_code());
00063 return 0;
00064 }
00065
00066 void WG014::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
00067 {
00068 stringstream str;
00069 str << "EtherCAT Device #" << setw(2) << setfill('0') << sh_->get_ring_position() << " (WG014)";
00070 d.name = str.str();
00071 d.summary(0, "OK");
00072 char serial[32];
00073 snprintf(serial, sizeof(serial), "%d-%05d-%05d", sh_->get_product_code()/ 100000 , sh_->get_product_code() % 100000, sh_->get_serial());
00074 d.hardware_id = serial;
00075
00076 d.clear();
00077 d.addf("Product code", "WG014 (%d), Ports %s, PCB Revision %c.%02d",
00078 sh_->get_product_code(),
00079 ((fw_minor_==1) ? "J1-J3" :
00080 (fw_minor_==2) ? "J4-J6" :
00081 "J?-J?"),
00082 'A' + board_major_, board_minor_);
00083 d.addf("Serial Number", "%s", serial);
00084
00085 EthercatDevice::ethercatDiagnostics(d, 4);
00086 }