wg021.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <iomanip>
36 
37 #include <math.h>
38 #include <stddef.h>
39 
41 
42 #include <dll/ethercat_dll.h>
43 #include <al/ethercat_AL.h>
44 #include <dll/ethercat_device_addressed_telegram.h>
45 #include <dll/ethercat_frame.h>
46 
47 #include <boost/static_assert.hpp>
48 
50 
52 
53 void WG021::construct(EtherCAT_SlaveHandler *sh, int &start_address)
54 {
55  WG0X::construct(sh, start_address);
56 
57  unsigned int base_status = sizeof(WG0XStatus);
58 
59  // As good a place as any for making sure that compiler actually packed these structures correctly
60  BOOST_STATIC_ASSERT(sizeof(WG021Status) == WG021Status::SIZE);
61 
62  status_size_ = base_status = sizeof(WG021Status);
63  command_size_ = sizeof(WG021Command);
64 
65  EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
66  //ROS_DEBUG("device %d, command 0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
67  (*fmmu)[0] = EC_FMMU(start_address, // Logical start address
68  command_size_,// Logical length
69  0x00, // Logical StartBit
70  0x07, // Logical EndBit
71  COMMAND_PHY_ADDR, // Physical Start address
72  0x00, // Physical StartBit
73  false, // Read Enable
74  true, // Write Enable
75  true); // Enable
76 
77  start_address += command_size_;
78 
79  //ROS_DEBUG("device %d, status 0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
80  (*fmmu)[1] = EC_FMMU(start_address, // Logical start address
81  base_status, // Logical length
82  0x00, // Logical StartBit
83  0x07, // Logical EndBit
84  STATUS_PHY_ADDR, // Physical Start address
85  0x00, // Physical StartBit
86  true, // Read Enable
87  false, // Write Enable
88  true); // Enable
89 
90  start_address += base_status;
91 
92  sh->set_fmmu_config(fmmu);
93 
94  EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(4);
95 
96  // Sync managers
97  (*pd)[0] = EC_SyncMan(COMMAND_PHY_ADDR, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
98  (*pd)[0].ChannelEnable = true;
99  (*pd)[0].ALEventEnable = true;
100 
101  (*pd)[1] = EC_SyncMan(STATUS_PHY_ADDR, base_status);
102  (*pd)[1].ChannelEnable = true;
103 
104  (*pd)[2] = EC_SyncMan(WGMailbox::MBX_COMMAND_PHY_ADDR, WGMailbox::MBX_COMMAND_SIZE, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
105  (*pd)[2].ChannelEnable = true;
106  (*pd)[2].ALEventEnable = true;
107 
108  (*pd)[3] = EC_SyncMan(WGMailbox::MBX_STATUS_PHY_ADDR, WGMailbox::MBX_STATUS_SIZE, EC_QUEUED);
109  (*pd)[3].ChannelEnable = true;
110 
111  sh->set_pd_config(pd);
112 }
113 
115 {
116  // WG021 has no use for application ram
118 
119  int retval = WG0X::initialize(hw, allow_unprogrammed);
120 
121  // Register digital outs with pr2_hardware_interface::HardwareInterface
122  struct {
124  string name;
125  } digital_outs[] = {
126  {&digital_out_A_, "_digital_out_A"},
127  {&digital_out_B_, "_digital_out_B"},
128  {&digital_out_I_, "_digital_out_I"},
129  {&digital_out_M_, "_digital_out_M"},
130  {&digital_out_L0_, "_digital_out_L0"},
131  {&digital_out_L1_, "_digital_out_L1"},
132  };
133 
134  for (size_t i = 0; i < sizeof(digital_outs)/sizeof(digital_outs[0]); ++i)
135  {
136  digital_outs[i].d->name_ = string(actuator_info_.name_) + digital_outs[i].name;
137  if (hw && !hw->addDigitalOut(digital_outs[i].d))
138  {
139  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());
140  return -1;
141  }
142  }
143 
144  // Register projector with pr2_hardware_interface::HardwareInterface
145  {
147  if (hw && !hw->addProjector(&projector_))
148  {
149  ROS_FATAL("A projector of the name '%s' already exists. Device #%02d has a duplicate name", projector_.name_.c_str(), sh_->get_ring_position());
150  return -1;
151  }
152  projector_.command_.enable_ = true;
154  }
155 
156  return retval;
157 }
158 
159 void WG021::packCommand(unsigned char *buffer, bool halt, bool reset)
160 {
162 
163  // Override enable if motors are halted
164  if (reset)
165  {
166  clearErrorFlags();
167  }
168  resetting_ = reset;
169 
170  // Truncate the current to limit (do not allow negative current)
172  cmd.current_ = max(min(cmd.current_, max_current_), 0.0);
173 
174  // Pack command structures into EtherCAT buffer
175  WG021Command *c = (WG021Command *)buffer;
176  memset(c, 0, command_size_);
179  c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
180  c->mode_ |= reset ? MODE_SAFETY_RESET : 0;
181  c->config0_ = ((cmd.A_ & 0xf) << 4) | ((cmd.B_ & 0xf) << 0);
182  c->config1_ = ((cmd.I_ & 0xf) << 4) | ((cmd.M_ & 0xf) << 0);
183  c->config2_ = ((cmd.L1_ & 0xf) << 4) | ((cmd.L0_ & 0xf) << 0);
184  c->general_config_ = cmd.pulse_replicator_ == true;
186 }
187 
188 bool WG021::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
189 {
190  bool rv = true;
191 
193  WG021Status *this_status;
194  //WG021Status *prev_status;
195  this_status = (WG021Status *)(this_buffer + command_size_);
196  //prev_status = (WG021Status *)(prev_buffer + command_size_);
197 
198  if (!verifyChecksum(this_status, status_size_))
199  {
200  status_checksum_error_ = true;
201  rv = false;
202  goto end;
203  }
204 
205  digital_out_.state_.data_ = this_status->digital_out_;
206 
207  state.timestamp_us_ = this_status->timestamp_;
208  state.falling_timestamp_us_ = this_status->output_stop_timestamp_;
209  state.rising_timestamp_us_ = this_status->output_start_timestamp_;
210 
211  state.output_ = (this_status->output_status_ & 0x1) == 0x1;
212  state.falling_timestamp_valid_ = (this_status->output_status_ & 0x8) == 0x8;
213  state.rising_timestamp_valid_ = (this_status->output_status_ & 0x4) == 0x4;
214 
215  state.A_ = ((this_status->config0_ >> 4) & 0xf);
216  state.B_ = ((this_status->config0_ >> 0) & 0xf);
217  state.I_ = ((this_status->config1_ >> 4) & 0xf);
218  state.M_ = ((this_status->config1_ >> 0) & 0xf);
219  state.L1_ = ((this_status->config2_ >> 4) & 0xf);
220  state.L0_ = ((this_status->config2_ >> 0) & 0xf);
221  state.pulse_replicator_ = (this_status->general_config_ & 0x1) == 0x1;
222 
225 
226  state.max_current_ = max_current_;
227 
230 
231  if (!verifyState((WG0XStatus *)(this_buffer + command_size_), (WG0XStatus *)(prev_buffer + command_size_)))
232  {
233  rv = false;
234  }
235 
236 end:
237  return rv;
238 }
239 
240 
241 
243 {
244  WG021Status *status = (WG021Status *)(buffer + command_size_);
245 
246  stringstream str;
247  str << "EtherCAT Device (" << actuator_info_.name_ << ")";
248  d.name = str.str();
249  char serial[32];
250  snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
251  d.hardware_id = serial;
252 
253  d.summary(d.OK, "OK");
254 
255  d.clear();
256  d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
257  d.add("Name", actuator_info_.name_);
258  d.addf("Position", "%02d", sh_->get_ring_position());
259  d.addf("Product code",
260  "WG021 (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
261  sh_->get_product_code(), fw_major_, fw_minor_,
262  'A' + board_major_, board_minor_);
263 
264  d.add("Robot", actuator_info_.robot_name_);
265  d.add("Serial Number", serial);
266  d.addf("Nominal Current Scale", "%f", config_info_.nominal_current_scale_);
267  d.addf("Nominal Voltage Scale", "%f", config_info_.nominal_voltage_scale_);
269  d.addf("SW Max Current", "%f", actuator_info_.max_current_);
270 
273 
274  d.add("Mode", modeString(status->mode_));
275  d.addf("Digital out", "%d", status->digital_out_);
276  d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
277  d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
278  d.addf("Timestamp", "%u", status->timestamp_);
279  d.addf("Config 0", "%#02x", status->config0_);
280  d.addf("Config 1", "%#02x", status->config1_);
281  d.addf("Config 2", "%#02x", status->config2_);
282  d.addf("Output Status", "%#02x", status->output_status_);
283  d.addf("Output Start Timestamp", "%u", status->output_start_timestamp_);
284  d.addf("Output Stop Timestamp", "%u", status->output_stop_timestamp_);
285  d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
286  d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
287  d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
288  d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
289  d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
290  d.addf("LED voltage", "%f", status->led_voltage_ * config_info_.nominal_voltage_scale_);
291  d.addf("Packet count", "%d", status->packet_count_);
292 
294 }
int16_t measured_current_
Definition: wg021.h:48
uint8_t mode_
Definition: wg021.h:72
uint8_t config2_
Definition: wg021.h:52
d
char name_[64]
Definition: wg0x.h:141
uint16_t max_bridge_temperature_
Definition: wg0x.h:296
#define ROS_FATAL(...)
unsigned int command_size_
pr2_hardware_interface::DigitalOut digital_out_L0_
Definition: wg021.h:121
void publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
Definition: wg_mailbox.cpp:993
void publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
Definition: wg0x.cpp:1223
bool resetting_
Definition: wg0x.h:294
static string modeString(uint8_t mode)
Definition: wg0x.cpp:1189
uint32_t product_id_
Definition: wg0x.h:98
uint8_t config2_
Definition: wg021.h:81
void summary(unsigned char lvl, const std::string s)
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg021.cpp:53
uint8_t config0_
Definition: wg021.h:79
string cmd
double max_current_
Definition: wg0x.h:145
unsigned computeChecksum(void const *data, unsigned length)
Definition: wg_util.cpp:49
void ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts)
Adds diagnostic information for EtherCAT ports.
pr2_hardware_interface::Projector projector_
Definition: wg021.h:123
char robot_name_[32]
Definition: wg0x.h:142
uint16_t board_temperature_
Definition: wg021.h:60
uint8_t config1_
Definition: wg021.h:51
bool has_error_
Definition: wg0x.h:295
pr2_hardware_interface::DigitalOut digital_out_B_
Definition: wg021.h:118
pr2_hardware_interface::DigitalOut digital_out_I_
Definition: wg021.h:119
uint8_t board_major_
Printed circuit board revision (for this value 0==&#39;A&#39;, 1==&#39;B&#39;)
Definition: wg0x.h:260
float nominal_voltage_scale_
Definition: wg0x.h:115
bool status_checksum_error_
Definition: wg0x.h:298
static const unsigned SIZE
Definition: wg021.h:67
static const unsigned STATUS_PHY_ADDR
Definition: wg0x.h:357
pr2_hardware_interface::DigitalOut digital_out_
Definition: wg0x.h:271
uint8_t board_minor_
Printed circuit assembly revision.
Definition: wg0x.h:261
void addf(const std::string &key, const char *format,...)
int16_t programmed_current_
Definition: wg021.h:76
void clearErrorFlags(void)
Definition: wg0x.cpp:549
bool verifyState(WG0XStatus *this_status, WG0XStatus *prev_status)
Definition: wg0x.cpp:749
int16_t led_voltage_
Definition: wg021.h:63
uint8_t digital_out_
Definition: wg021.h:73
unsigned int rotateRight8(unsigned in)
Definition: wg_util.cpp:41
uint8_t mode_
Definition: wg021.h:43
uint8_t general_config_
Definition: wg021.h:45
uint16_t absolute_current_limit_
Definition: wg0x.h:113
uint16_t status
uint8_t digital_out_
Definition: wg021.h:44
uint8_t config0_
Definition: wg021.h:50
uint8_t configuration_status_
Definition: wg0x.h:117
Definition: wg021.h:85
WG0XConfigInfo config_info_
Definition: wg0x.h:264
double max_current_
min(board current limit, actuator current limit)
Definition: wg0x.h:265
EtherCAT_SlaveHandler * sh_
bool addProjector(Projector *projector)
pr2_hardware_interface::DigitalOut digital_out_A_
Definition: wg021.h:117
uint16_t bridge_temperature_
Definition: wg021.h:61
uint8_t fw_minor_
Definition: wg0x.h:259
uint8_t checksum_
Definition: wg021.h:82
PLUGINLIB_EXPORT_CLASS(WG021, EthercatDevice)
AppRamStatus app_ram_status_
Definition: wg0x.h:324
uint8_t output_status_
Definition: wg021.h:57
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg0x.cpp:198
uint32_t device_serial_number_
Definition: wg0x.h:110
float nominal_current_scale_
Definition: wg0x.h:114
uint16_t packet_count_
Definition: wg021.h:64
uint16_t max_board_temperature_
Definition: wg0x.h:296
unsigned int status_size_
void packCommand(unsigned char *buffer, bool halt, bool reset)
Definition: wg021.cpp:159
int min(int a, int b)
int16_t programmed_current_
Definition: wg021.h:47
uint8_t fw_major_
Definition: wg0x.h:258
uint8_t general_config_
Definition: wg021.h:74
uint32_t timestamp_
Definition: wg021.h:49
bool verifyChecksum(const void *buffer, unsigned size)
Definition: wg0x.cpp:665
pr2_hardware_interface::DigitalOut digital_out_M_
Definition: wg021.h:120
pr2_hardware_interface::DigitalOut digital_out_L1_
Definition: wg021.h:122
uint16_t supply_voltage_
Definition: wg021.h:62
ethercat_hardware::WGMailbox mailbox_
Mailbox access to device.
Definition: wg0x.h:351
uint8_t config1_
Definition: wg021.h:80
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
For EtherCAT device that only publish one EtherCAT Status message. If sub-class implements multiDiagn...
Definition: wg021.cpp:242
static const unsigned COMMAND_PHY_ADDR
Definition: wg0x.h:356
void add(const std::string &key, const T &val)
uint32_t output_start_timestamp_
Definition: wg021.h:58
int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
Definition: wg021.cpp:114
bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
Definition: wg021.cpp:188
uint32_t output_stop_timestamp_
Definition: wg021.h:59
virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
Definition: wg0x.cpp:395
bool addDigitalOut(DigitalOut *digital_out)
WG0XActuatorInfo actuator_info_
Definition: wg0x.h:263


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Tue Mar 28 2023 02:10:20