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 <iomanip>
00036
00037 #include <math.h>
00038 #include <stddef.h>
00039
00040 #include <ethercat_hardware/wg021.h>
00041
00042 #include <dll/ethercat_dll.h>
00043 #include <al/ethercat_AL.h>
00044 #include <dll/ethercat_device_addressed_telegram.h>
00045 #include <dll/ethercat_frame.h>
00046
00047 #include <boost/crc.hpp>
00048 #include <boost/static_assert.hpp>
00049
00050 #include "ethercat_hardware/wg_util.h"
00051
00052 PLUGINLIB_DECLARE_CLASS(ethercat_hardware, 6805021, WG021, EthercatDevice);
00053
00054 void WG021::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00055 {
00056 WG0X::construct(sh, start_address);
00057
00058 unsigned int base_status = sizeof(WG0XStatus);
00059
00060
00061 BOOST_STATIC_ASSERT(sizeof(WG021Status) == WG021Status::SIZE);
00062
00063 status_size_ = base_status = sizeof(WG021Status);
00064 command_size_ = sizeof(WG021Command);
00065
00066 EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00067
00068 (*fmmu)[0] = EC_FMMU(start_address,
00069 command_size_,
00070 0x00,
00071 0x07,
00072 COMMAND_PHY_ADDR,
00073 0x00,
00074 false,
00075 true,
00076 true);
00077
00078 start_address += command_size_;
00079
00080
00081 (*fmmu)[1] = EC_FMMU(start_address,
00082 base_status,
00083 0x00,
00084 0x07,
00085 STATUS_PHY_ADDR,
00086 0x00,
00087 true,
00088 false,
00089 true);
00090
00091 start_address += base_status;
00092
00093 sh->set_fmmu_config(fmmu);
00094
00095 EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(4);
00096
00097
00098 (*pd)[0] = EC_SyncMan(COMMAND_PHY_ADDR, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
00099 (*pd)[0].ChannelEnable = true;
00100 (*pd)[0].ALEventEnable = true;
00101
00102 (*pd)[1] = EC_SyncMan(STATUS_PHY_ADDR, base_status);
00103 (*pd)[1].ChannelEnable = true;
00104
00105 (*pd)[2] = EC_SyncMan(WGMailbox::MBX_COMMAND_PHY_ADDR, WGMailbox::MBX_COMMAND_SIZE, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
00106 (*pd)[2].ChannelEnable = true;
00107 (*pd)[2].ALEventEnable = true;
00108
00109 (*pd)[3] = EC_SyncMan(WGMailbox::MBX_STATUS_PHY_ADDR, WGMailbox::MBX_STATUS_SIZE, EC_QUEUED);
00110 (*pd)[3].ChannelEnable = true;
00111
00112 sh->set_pd_config(pd);
00113 }
00114
00115 int WG021::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00116 {
00117
00118 app_ram_status_ = APP_RAM_NOT_APPLICABLE;
00119
00120 int retval = WG0X::initialize(hw, allow_unprogrammed);
00121
00122
00123 struct {
00124 pr2_hardware_interface::DigitalOut *d;
00125 string name;
00126 } digital_outs[] = {
00127 {&digital_out_A_, "_digital_out_A"},
00128 {&digital_out_B_, "_digital_out_B"},
00129 {&digital_out_I_, "_digital_out_I"},
00130 {&digital_out_M_, "_digital_out_M"},
00131 {&digital_out_L0_, "_digital_out_L0"},
00132 {&digital_out_L1_, "_digital_out_L1"},
00133 };
00134
00135 for (size_t i = 0; i < sizeof(digital_outs)/sizeof(digital_outs[0]); ++i)
00136 {
00137 digital_outs[i].d->name_ = string(actuator_info_.name_) + digital_outs[i].name;
00138 if (hw && !hw->addDigitalOut(digital_outs[i].d))
00139 {
00140 ROS_FATAL("A digital out of the name '%s' already exists. Device #%02d has a duplicate name", digital_outs[i].d->name_.c_str(), sh_->get_ring_position());
00141 return -1;
00142 }
00143 }
00144
00145
00146 {
00147 projector_.name_ = actuator_info_.name_;
00148 if (hw && !hw->addProjector(&projector_))
00149 {
00150 ROS_FATAL("A projector of the name '%s' already exists. Device #%02d has a duplicate name", projector_.name_.c_str(), sh_->get_ring_position());
00151 return -1;
00152 }
00153 projector_.command_.enable_ = true;
00154 projector_.command_.current_ = 0;
00155 }
00156
00157 return retval;
00158 }
00159
00160 void WG021::packCommand(unsigned char *buffer, bool halt, bool reset)
00161 {
00162 pr2_hardware_interface::ProjectorCommand &cmd = projector_.command_;
00163
00164
00165 if (reset)
00166 {
00167 clearErrorFlags();
00168 }
00169 resetting_ = reset;
00170
00171
00172 projector_.state_.last_commanded_current_ = cmd.current_;
00173 cmd.current_ = max(min(cmd.current_, max_current_), 0.0);
00174
00175
00176 WG021Command *c = (WG021Command *)buffer;
00177 memset(c, 0, command_size_);
00178 c->digital_out_ = digital_out_.command_.data_;
00179 c->programmed_current_ = int(cmd.current_ / config_info_.nominal_current_scale_);
00180 c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
00181 c->mode_ |= reset ? MODE_SAFETY_RESET : 0;
00182 c->config0_ = ((cmd.A_ & 0xf) << 4) | ((cmd.B_ & 0xf) << 0);
00183 c->config1_ = ((cmd.I_ & 0xf) << 4) | ((cmd.M_ & 0xf) << 0);
00184 c->config2_ = ((cmd.L1_ & 0xf) << 4) | ((cmd.L0_ & 0xf) << 0);
00185 c->general_config_ = cmd.pulse_replicator_ == true;
00186 c->checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(c, command_size_ - 1));
00187 }
00188
00189 bool WG021::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00190 {
00191 bool rv = true;
00192
00193 pr2_hardware_interface::ProjectorState &state = projector_.state_;
00194 WG021Status *this_status, *prev_status;
00195 this_status = (WG021Status *)(this_buffer + command_size_);
00196 prev_status = (WG021Status *)(prev_buffer + command_size_);
00197
00198 if (!verifyChecksum(this_status, status_size_))
00199 {
00200 status_checksum_error_ = true;
00201 rv = false;
00202 goto end;
00203 }
00204
00205 digital_out_.state_.data_ = this_status->digital_out_;
00206
00207 state.timestamp_us_ = this_status->timestamp_;
00208 state.falling_timestamp_us_ = this_status->output_stop_timestamp_;
00209 state.rising_timestamp_us_ = this_status->output_start_timestamp_;
00210
00211 state.output_ = (this_status->output_status_ & 0x1) == 0x1;
00212 state.falling_timestamp_valid_ = (this_status->output_status_ & 0x8) == 0x8;
00213 state.rising_timestamp_valid_ = (this_status->output_status_ & 0x4) == 0x4;
00214
00215 state.A_ = ((this_status->config0_ >> 4) & 0xf);
00216 state.B_ = ((this_status->config0_ >> 0) & 0xf);
00217 state.I_ = ((this_status->config1_ >> 4) & 0xf);
00218 state.M_ = ((this_status->config1_ >> 0) & 0xf);
00219 state.L1_ = ((this_status->config2_ >> 4) & 0xf);
00220 state.L0_ = ((this_status->config2_ >> 0) & 0xf);
00221 state.pulse_replicator_ = (this_status->general_config_ & 0x1) == 0x1;
00222
00223 state.last_executed_current_ = this_status->programmed_current_ * config_info_.nominal_current_scale_;
00224 state.last_measured_current_ = this_status->measured_current_ * config_info_.nominal_current_scale_;
00225
00226 state.max_current_ = max_current_;
00227
00228 max_board_temperature_ = max(max_board_temperature_, this_status->board_temperature_);
00229 max_bridge_temperature_ = max(max_bridge_temperature_, this_status->bridge_temperature_);
00230
00231 if (!verifyState((WG0XStatus *)(this_buffer + command_size_), (WG0XStatus *)(prev_buffer + command_size_)))
00232 {
00233 rv = false;
00234 }
00235
00236 end:
00237 return rv;
00238 }
00239
00240
00241
00242 void WG021::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00243 {
00244 WG021Status *status = (WG021Status *)(buffer + command_size_);
00245
00246 stringstream str;
00247 str << "EtherCAT Device (" << actuator_info_.name_ << ")";
00248 d.name = str.str();
00249 char serial[32];
00250 snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
00251 d.hardware_id = serial;
00252
00253 d.summary(d.OK, "OK");
00254
00255 d.clear();
00256 d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
00257 d.add("Name", actuator_info_.name_);
00258 d.addf("Position", "%02d", sh_->get_ring_position());
00259 d.addf("Product code",
00260 "WG021 (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
00261 sh_->get_product_code(), fw_major_, fw_minor_,
00262 'A' + board_major_, board_minor_);
00263
00264 d.add("Robot", actuator_info_.robot_name_);
00265 d.add("Serial Number", serial);
00266 d.addf("Nominal Current Scale", "%f", config_info_.nominal_current_scale_);
00267 d.addf("Nominal Voltage Scale", "%f", config_info_.nominal_voltage_scale_);
00268 d.addf("HW Max Current", "%f", config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_);
00269 d.addf("SW Max Current", "%f", actuator_info_.max_current_);
00270
00271 publishGeneralDiagnostics(d);
00272 mailbox_.publishMailboxDiagnostics(d);
00273
00274 d.add("Mode", modeString(status->mode_));
00275 d.addf("Digital out", "%d", status->digital_out_);
00276 d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
00277 d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
00278 d.addf("Timestamp", "%u", status->timestamp_);
00279 d.addf("Config 0", "%#02x", status->config0_);
00280 d.addf("Config 1", "%#02x", status->config1_);
00281 d.addf("Config 2", "%#02x", status->config2_);
00282 d.addf("Output Status", "%#02x", status->output_status_);
00283 d.addf("Output Start Timestamp", "%u", status->output_start_timestamp_);
00284 d.addf("Output Stop Timestamp", "%u", status->output_stop_timestamp_);
00285 d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
00286 d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
00287 d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
00288 d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
00289 d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
00290 d.addf("LED voltage", "%f", status->led_voltage_ * config_info_.nominal_voltage_scale_);
00291 d.addf("Packet count", "%d", status->packet_count_);
00292
00293 EthercatDevice::ethercatDiagnostics(d, 2);
00294 }