wg_soft_processor.cpp
Go to the documentation of this file.
2 
3 #include <sstream>
4 #include <boost/foreach.hpp>
5 #include <boost/static_assert.hpp>
6 
7 namespace ethercat_hardware
8 {
9 
11 {
12 
13 }
14 
16 {
17  com_ = com;
18  ros::NodeHandle nh("~/soft_processor/");
22  return true;
23 }
24 
25 
27  const std::string &actuator_name,
28  const std::string &processor_name,
29  unsigned iram_address, unsigned ctrl_address)
30 {
31  Info info(mbx, actuator_name, processor_name, iram_address, ctrl_address);
32  processors_.push_back(info);
33  ROS_INFO("Processor : %s/%s", actuator_name.c_str(), processor_name.c_str());
34 }
35 
36 
37 
38 const WGSoftProcessor::Info* WGSoftProcessor::get(const std::string &actuator_name,
39  const std::string &processor_name,
40  std::ostream &err_out) const
41 {
42  BOOST_FOREACH(const Info &info, processors_)
43  {
44  if ((info.actuator_name_ == actuator_name) && (info.processor_name_ == processor_name))
45  {
46  return &info;
47  }
48  }
49 
50  err_out << "No actuator/processor with name " << actuator_name << "/" << processor_name;
51  return NULL;
52 }
53 
54 
55 
56 bool WGSoftProcessor::readFirmwareCB(ethercat_hardware::SoftProcessorFirmwareRead::Request &request,
57  ethercat_hardware::SoftProcessorFirmwareRead::Response &response)
58 {
59  response.success = false;
60  response.error_msg = "";
61 
62  std::ostringstream err_out;
63 
64  const Info *info = get(request.actuator_name, request.processor_name, err_out);
65  if (!info)
66  {
67  response.error_msg = err_out.str();
68  return true;
69  }
70 
71  // Each instruction is maped to 32bit memory array. Read 64 instructions at a time.
72  response.instructions.resize(IRAM_INSTRUCTION_LENGTH);
73  static const unsigned INSTRUCTION_READ_CHUNK = 64;
74  uint8_t buf[INSTRUCTION_READ_CHUNK*4]; // Each instruction in 4 bytes
75 
76  BOOST_STATIC_ASSERT((IRAM_INSTRUCTION_LENGTH%INSTRUCTION_READ_CHUNK) == 0);
77 
78  for (unsigned ii=0; ii<(IRAM_INSTRUCTION_LENGTH/INSTRUCTION_READ_CHUNK); ii+=INSTRUCTION_READ_CHUNK)
79  {
80  if (info->mbx_->readMailbox(com_, info->iram_address_ + ii*4, buf, sizeof(buf)))
81  {
82  response.error_msg = "Error reading IRAM data with mailbox";
83  return true;
84  }
85 
86  // Data 4-bytes and make integer out of them
87  for (unsigned jj=0; jj<INSTRUCTION_READ_CHUNK; ++jj)
88  {
89  // instrutions are little endian
90  uint32_t instruction =
91  (uint32_t(buf[jj*4+3])<<24) |
92  (uint32_t(buf[jj*4+2])<<16) |
93  (uint32_t(buf[jj*4+1])<<8 ) |
94  (uint32_t(buf[jj*4+0])<<0 ) ;
95 
96  response.instructions[ii+jj] = instruction;
97  }
98  }
99 
100  response.success = true;
101  return true;
102 }
103 
104 
105 
106 bool WGSoftProcessor::writeFirmwareCB(ethercat_hardware::SoftProcessorFirmwareWrite::Request &request,
107  ethercat_hardware::SoftProcessorFirmwareWrite::Response &response)
108 {
109  response.success = false;
110  response.error_msg = "";
111 
112  std::ostringstream err_out;
113 
114  const Info *info = get(request.actuator_name, request.processor_name, err_out);
115  if (!info)
116  {
117  response.error_msg = err_out.str();
118  return true;
119  }
120 
121  // Put soft-processor in reset before starting to re-write firmware
122  if (!assertReset(*info, err_out))
123  {
124  response.error_msg = err_out.str();
125  return true;
126  }
127 
128  // perform write here
129 
130 
131  // Take soft-processor out of reset now that firmware is completely re-written
132  if (!releaseReset(*info, err_out))
133  {
134  response.error_msg = err_out.str();
135  return true;
136  }
137 
138  response.success = true;
139  return true;
140 }
141 
142 
143 bool WGSoftProcessor::resetCB(ethercat_hardware::SoftProcessorReset::Request &request,
144  ethercat_hardware::SoftProcessorReset::Response &response)
145 {
146  response.success = false;
147  response.error_msg = "";
148 
149  std::ostringstream err_out;
150 
151  const Info *info = get(request.actuator_name, request.processor_name, err_out);
152  if (!info)
153  {
154  response.error_msg = err_out.str();
155  return true;
156  }
157 
158  if (!assertReset(*info, err_out))
159  {
160  response.error_msg = err_out.str();
161  return true;
162  }
163 
164  if (!releaseReset(*info, err_out))
165  {
166  response.error_msg = err_out.str();
167  return true;
168  }
169 
170  response.success = true;
171  return true;
172 }
173 
174 
176 bool WGSoftProcessor::assertReset(const Info &info, std::ostream &err_msg)
177 {
178  // use mailbox to set bit 0 of byte at info->ctrl_address
179 
180  return true;
181 }
182 
184 bool WGSoftProcessor::releaseReset(const Info &info, std::ostream &err_msg)
185 {
186  // use mailbox to clear bit 0 of byte at info->ctrl_address
187 
188  return true;
189 }
190 
191 
192 
193 
194 }; // end namespace ethercat_hardware
195 
void add(WGMailbox *mbx, const std::string &actuator_name, const std::string &processor_name, unsigned iram_address, unsigned ctrl_address)
bool assertReset(const Info &info, std::ostream &err_msg)
Puts soft processor in reset.
bool resetCB(ethercat_hardware::SoftProcessorReset::Request &request, ethercat_hardware::SoftProcessorReset::Response &response)
const WGSoftProcessor::Info * get(const std::string &actuator_name, const std::string &processor_name, std::ostream &err_out) const
Get pointer to soft processor by name. Returns NULL if processor d/n exist and create message in err_...
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::ServiceServer write_firmware_service_
service that allows write of soft processor firmware
bool writeFirmwareCB(ethercat_hardware::SoftProcessorFirmwareWrite::Request &request, ethercat_hardware::SoftProcessorFirmwareWrite::Response &response)
#define ROS_INFO(...)
ros::ServiceServer reset_service_
service that resets soft-processor
static const unsigned IRAM_INSTRUCTION_LENGTH
int readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length)
Read data from WG0X local bus using mailbox communication.
Definition: wg_mailbox.cpp:812
bool readFirmwareCB(ethercat_hardware::SoftProcessorFirmwareRead::Request &request, ethercat_hardware::SoftProcessorFirmwareRead::Response &response)
bool releaseReset(const Info &info, std::ostream &err_msg)
Takes soft processor out of reset.
ros::ServiceServer read_firmware_service_
service that allows read of soft processor firmware


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Mar 4 2021 03:10:21