wg_soft_processor.cpp
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   // Each instruction is maped to 32bit memory array.  Read 64 instructions at a time.
00072   response.instructions.resize(IRAM_INSTRUCTION_LENGTH);
00073   static const unsigned INSTRUCTION_READ_CHUNK = 64;
00074   uint8_t buf[INSTRUCTION_READ_CHUNK*4]; // Each instruction in 4 bytes
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     // Data 4-bytes and make integer out of them
00087     for (unsigned jj=0; jj<INSTRUCTION_READ_CHUNK; ++jj)
00088     {     
00089       // instrutions are little endian
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   // Put soft-processor in reset before starting to re-write firmware
00122   if (!assertReset(*info, err_out))
00123   {
00124     response.error_msg = err_out.str();    
00125     return true;
00126   }
00127 
00128   // perform write here
00129 
00130 
00131   // Take soft-processor out of reset now that firmware is completely re-written
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   // use mailbox to set bit 0 of byte at info->ctrl_address
00179 
00180   return true;
00181 }
00182 
00184 bool WGSoftProcessor::releaseReset(const Info &info, std::ostream &err_msg)
00185 {
00186   // use mailbox to clear bit 0 of byte at info->ctrl_address
00187   
00188   return true;
00189 }
00190 
00191 
00192 
00193 
00194 }; // end namespace ethercat_hardware
00195 


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:45