Go to the documentation of this file.00001 #include <ethercat_hardware/wg_soft_processor.h>
00002
00003 #include <sstream>
00004 #include <boost/foreach.hpp>
00005 #include <boost/static_assert.hpp>
00006
00007 namespace ethercat_hardware
00008 {
00009
00010 WGSoftProcessor::WGSoftProcessor()
00011 {
00012
00013 }
00014
00015 bool WGSoftProcessor::initialize(EthercatCom *com)
00016 {
00017 com_ = com;
00018 ros::NodeHandle nh("~/soft_processor/");
00019 read_firmware_service_ = nh.advertiseService("read_firmware", &WGSoftProcessor::readFirmwareCB, this);
00020 write_firmware_service_ = nh.advertiseService("write_firmware", &WGSoftProcessor::writeFirmwareCB, this);
00021 reset_service_ = nh.advertiseService("reset", &WGSoftProcessor::resetCB, this);
00022 return true;
00023 }
00024
00025
00026 void WGSoftProcessor::add(WGMailbox *mbx,
00027 const std::string &actuator_name,
00028 const std::string &processor_name,
00029 unsigned iram_address, unsigned ctrl_address)
00030 {
00031 Info info(mbx, actuator_name, processor_name, iram_address, ctrl_address);
00032 processors_.push_back(info);
00033 ROS_INFO("Processor : %s/%s", actuator_name.c_str(), processor_name.c_str());
00034 }
00035
00036
00037
00038 const WGSoftProcessor::Info* WGSoftProcessor::get(const std::string &actuator_name,
00039 const std::string &processor_name,
00040 std::ostream &err_out) const
00041 {
00042 BOOST_FOREACH(const Info &info, processors_)
00043 {
00044 if ((info.actuator_name_ == actuator_name) && (info.processor_name_ == processor_name))
00045 {
00046 return &info;
00047 }
00048 }
00049
00050 err_out << "No actuator/processor with name " << actuator_name << "/" << processor_name;
00051 return NULL;
00052 }
00053
00054
00055
00056 bool WGSoftProcessor::readFirmwareCB(ethercat_hardware::SoftProcessorFirmwareRead::Request &request,
00057 ethercat_hardware::SoftProcessorFirmwareRead::Response &response)
00058 {
00059 response.success = false;
00060 response.error_msg = "";
00061
00062 std::ostringstream err_out;
00063
00064 const Info *info = get(request.actuator_name, request.processor_name, err_out);
00065 if (!info)
00066 {
00067 response.error_msg = err_out.str();
00068 return true;
00069 }
00070
00071
00072 response.instructions.resize(IRAM_INSTRUCTION_LENGTH);
00073 static const unsigned INSTRUCTION_READ_CHUNK = 64;
00074 uint8_t buf[INSTRUCTION_READ_CHUNK*4];
00075
00076 BOOST_STATIC_ASSERT((IRAM_INSTRUCTION_LENGTH%INSTRUCTION_READ_CHUNK) == 0);
00077
00078 for (unsigned ii=0; ii<(IRAM_INSTRUCTION_LENGTH/INSTRUCTION_READ_CHUNK); ii+=INSTRUCTION_READ_CHUNK)
00079 {
00080 if (info->mbx_->readMailbox(com_, info->iram_address_ + ii*4, buf, sizeof(buf)))
00081 {
00082 response.error_msg = "Error reading IRAM data with mailbox";
00083 return true;
00084 }
00085
00086
00087 for (unsigned jj=0; jj<INSTRUCTION_READ_CHUNK; ++jj)
00088 {
00089
00090 uint32_t instruction =
00091 (uint32_t(buf[jj*4+3])<<24) |
00092 (uint32_t(buf[jj*4+2])<<16) |
00093 (uint32_t(buf[jj*4+1])<<8 ) |
00094 (uint32_t(buf[jj*4+0])<<0 ) ;
00095
00096 response.instructions[ii+jj] = instruction;
00097 }
00098 }
00099
00100 response.success = true;
00101 return true;
00102 }
00103
00104
00105
00106 bool WGSoftProcessor::writeFirmwareCB(ethercat_hardware::SoftProcessorFirmwareWrite::Request &request,
00107 ethercat_hardware::SoftProcessorFirmwareWrite::Response &response)
00108 {
00109 response.success = false;
00110 response.error_msg = "";
00111
00112 std::ostringstream err_out;
00113
00114 const Info *info = get(request.actuator_name, request.processor_name, err_out);
00115 if (!info)
00116 {
00117 response.error_msg = err_out.str();
00118 return true;
00119 }
00120
00121
00122 if (!assertReset(*info, err_out))
00123 {
00124 response.error_msg = err_out.str();
00125 return true;
00126 }
00127
00128
00129
00130
00131
00132 if (!releaseReset(*info, err_out))
00133 {
00134 response.error_msg = err_out.str();
00135 return true;
00136 }
00137
00138 response.success = true;
00139 return true;
00140 }
00141
00142
00143 bool WGSoftProcessor::resetCB(ethercat_hardware::SoftProcessorReset::Request &request,
00144 ethercat_hardware::SoftProcessorReset::Response &response)
00145 {
00146 response.success = false;
00147 response.error_msg = "";
00148
00149 std::ostringstream err_out;
00150
00151 const Info *info = get(request.actuator_name, request.processor_name, err_out);
00152 if (!info)
00153 {
00154 response.error_msg = err_out.str();
00155 return true;
00156 }
00157
00158 if (!assertReset(*info, err_out))
00159 {
00160 response.error_msg = err_out.str();
00161 return true;
00162 }
00163
00164 if (!releaseReset(*info, err_out))
00165 {
00166 response.error_msg = err_out.str();
00167 return true;
00168 }
00169
00170 response.success = true;
00171 return true;
00172 }
00173
00174
00176 bool WGSoftProcessor::assertReset(const Info &info, std::ostream &err_msg)
00177 {
00178
00179
00180 return true;
00181 }
00182
00184 bool WGSoftProcessor::releaseReset(const Info &info, std::ostream &err_msg)
00185 {
00186
00187
00188 return true;
00189 }
00190
00191
00192
00193
00194 };
00195