sr_ronex_controllers: spi_passthrough_controller.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Shadow Robot Company, All rights reserved.
00003  *
00004  * This library is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU Lesser General Public
00006  * License as published by the Free Software Foundation; either
00007  * version 3.0 of the License, or (at your option) any later version.
00008  *
00009  * This library is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
00012  * Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public
00015  * License along with this library.
00016  */
00017 
00025 #include "sr_ronex_controllers/spi_passthrough_controller.hpp"
00026 #include "pluginlib/class_list_macros.h"
00027 
00028 PLUGINLIB_EXPORT_CLASS( ronex::SPIPassthroughController, controller_interface::ControllerBase)
00029 
00030 namespace ronex
00031 {
00032   bool SPIPassthroughController::init(ros_ethercat_model::RobotState* robot, ros::NodeHandle &n)
00033   {
00034     if( !pre_init_(robot, n) )
00035       return false;
00036 
00037     standard_commands_.assign(NUM_SPI_OUTPUTS, SplittedSPICommand());
00038     for(size_t i = 0; i < NUM_SPI_OUTPUTS; ++i)
00039     {
00040       std::ostringstream service_path;
00041       service_path << topic_prefix_ << "/command/passthrough/"<<i;
00042 
00043       command_srv_.push_back( node_.advertiseService<sr_ronex_msgs::SPI::Request, sr_ronex_msgs::SPI::Response>(service_path.str(), boost::bind(&SPIPassthroughController::command_srv_cb, this, _1, _2,  i)) );
00044     }
00045 
00046     dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<sr_ronex_drivers::SPIConfig>(ros::NodeHandle(topic_prefix_)));
00047     function_cb_ = boost::bind(&SPIPassthroughController::dynamic_reconfigure_cb, this, _1, _2);
00048     dynamic_reconfigure_server_->setCallback(function_cb_);
00049 
00050     return true;
00051   }
00052 
00053   bool SPIPassthroughController::command_srv_cb( sr_ronex_msgs::SPI::Request &req,
00054                                                  sr_ronex_msgs::SPI::Response &res,
00055                                                  size_t spi_out_index )
00056   {
00057     //transmitting the bytes we received
00058     standard_commands_[spi_out_index].packet.num_bytes = static_cast<int8u>(req.data.size());
00059 
00060     ROS_DEBUG_STREAM("From passthrough: received "<< req.data.size()<<"bytes.");
00061     for(size_t i = 0; i < req.data.size(); ++i)
00062     {
00063       try
00064       {
00065         standard_commands_[spi_out_index].packet.data_bytes[i] = static_cast<int8u>(req.data[i]);
00066       }
00067       catch(...)
00068       {
00069         ROS_ERROR_STREAM("Input["<<i<<"]: "<<req.data[i] << " could not be converted to int");
00070       }
00071     }
00072 
00073     //pushing to the command queue to be sent through etherCAT
00074     command_queue_[spi_out_index].push(&standard_commands_[spi_out_index]);
00075 
00076     //wait for the response to be received
00077     bool not_received = true;
00078     while( not_received )
00079     {
00080       //sleep roughly 1ms to wait for new etherCAT packet to be received.
00081       usleep(1000);
00082 
00083       for(size_t i = 0; i < status_queue_.size(); ++i)
00084       {
00085         if(!status_queue_[i].empty())
00086         {
00087           if( status_queue_[i].front().first == &standard_commands_[spi_out_index] )
00088           {
00089             if( status_queue_[i].front().second != NULL)
00090             {
00091               //found the status command corresponding to the command we sent
00092               // updating the response
00093               for(size_t j = 0; j < req.data.size(); ++j)
00094               {
00095                 std::ostringstream hex;
00096                 try
00097                 {
00098                   hex << static_cast<unsigned int>(status_queue_[i].front().second->data_bytes[j]);
00099                 }
00100                   catch(...)
00101                 {
00102                   ROS_ERROR_STREAM("Can't cast to uint.");
00103                   hex << "bad_data";
00104                 }
00105                 res.data.push_back(hex.str());
00106               }
00107               not_received = false;
00108 
00109               //we used the status (sent it back to the user through the service
00110               // response -> popping from the queue
00111               status_queue_[i].pop();
00112 
00113               break;
00114             }
00115           }
00116         }
00117       }
00118     }
00119     return true;
00120   }
00121 
00122   void SPIPassthroughController::dynamic_reconfigure_cb(sr_ronex_drivers::SPIConfig &config, uint32_t level)
00123   {
00124     spi_->command_->command_type = static_cast<int16u>(config.command_type);
00125 
00126     //setting up spi 0
00127     standard_commands_[0].packet.clock_divider = static_cast<int16u>(config.spi_0_clock_divider);
00128     standard_commands_[0].packet.SPI_config = static_cast<int16u>(config.spi_0_SPI_config);
00129     standard_commands_[0].packet.inter_byte_gap = static_cast<int16u>(config.spi_0_inter_byte_gap);
00130 
00131     //setting up spi 1
00132     standard_commands_[1].packet.clock_divider = static_cast<int16u>(config.spi_1_clock_divider);
00133     standard_commands_[1].packet.SPI_config = static_cast<int16u>(config.spi_1_SPI_config);
00134     standard_commands_[1].packet.inter_byte_gap = static_cast<int16u>(config.spi_1_inter_byte_gap);
00135 
00136     //setting up spi 2
00137     standard_commands_[2].packet.clock_divider = static_cast<int16u>(config.spi_2_clock_divider);
00138     standard_commands_[2].packet.SPI_config = static_cast<int16u>(config.spi_2_SPI_config);
00139     standard_commands_[2].packet.inter_byte_gap = static_cast<int16u>(config.spi_2_inter_byte_gap);
00140 
00141     //setting up spi 3
00142     standard_commands_[3].packet.clock_divider = static_cast<int16u>(config.spi_3_clock_divider);
00143     standard_commands_[3].packet.SPI_config = static_cast<int16u>(config.spi_3_SPI_config);
00144     standard_commands_[3].packet.inter_byte_gap = static_cast<int16u>(config.spi_3_inter_byte_gap);
00145 
00146 
00147     if( config.pin_output_state_pre_DIO_0 )
00148       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_0;
00149     else
00150       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_0;
00151 
00152     if( config.pin_output_state_post_DIO_0 )
00153       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_0;
00154     else
00155       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_0;
00156 
00157     if( config.pin_output_state_pre_dir_DIO_0 )
00158       cmd_pin_output_states_pre_ |= PIN_OUTPUT_DIRECTION_DIO_0;
00159     else
00160       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_0;
00161 
00162     if( config.pin_output_state_post_dir_DIO_0 )
00163       cmd_pin_output_states_post_ |= PIN_OUTPUT_DIRECTION_DIO_0;
00164     else
00165       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_0;
00166 
00167 
00168     if( config.pin_output_state_pre_DIO_1 )
00169       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_1;
00170     else
00171       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_1;
00172 
00173     if( config.pin_output_state_post_DIO_1 )
00174       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_1;
00175     else
00176       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_1;
00177 
00178     if( config.pin_output_state_pre_dir_DIO_1 )
00179       cmd_pin_output_states_pre_ |= PIN_OUTPUT_DIRECTION_DIO_1;
00180     else
00181       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_1;
00182 
00183     if( config.pin_output_state_post_dir_DIO_1 )
00184       cmd_pin_output_states_post_ |= PIN_OUTPUT_DIRECTION_DIO_1;
00185     else
00186       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_1;
00187 
00188 
00189     if( config.pin_output_state_pre_DIO_2 )
00190       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_2;
00191     else
00192       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_2;
00193 
00194     if( config.pin_output_state_post_DIO_2 )
00195       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_2;
00196     else
00197       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_2;
00198 
00199     if( config.pin_output_state_pre_dir_DIO_2 )
00200       cmd_pin_output_states_pre_ |= PIN_OUTPUT_DIRECTION_DIO_2;
00201     else
00202       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_2;
00203 
00204     if( config.pin_output_state_post_dir_DIO_2 )
00205       cmd_pin_output_states_post_ |= PIN_OUTPUT_DIRECTION_DIO_2;
00206     else
00207       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_2;
00208 
00209 
00210     if( config.pin_output_state_pre_DIO_3 )
00211       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_3;
00212     else
00213       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_3;
00214 
00215     if( config.pin_output_state_post_DIO_3 )
00216       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_3;
00217     else
00218       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_3;
00219 
00220     if( config.pin_output_state_pre_dir_DIO_3 )
00221       cmd_pin_output_states_pre_ |= PIN_OUTPUT_DIRECTION_DIO_3;
00222     else
00223       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_3;
00224 
00225     if( config.pin_output_state_post_dir_DIO_3 )
00226       cmd_pin_output_states_post_ |= PIN_OUTPUT_DIRECTION_DIO_3;
00227     else
00228       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_3;
00229 
00230 
00231     if( config.pin_output_state_pre_DIO_4 )
00232       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_4;
00233     else
00234       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_4;
00235 
00236     if( config.pin_output_state_post_DIO_4 )
00237       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_4;
00238     else
00239       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_4;
00240 
00241     if( config.pin_output_state_pre_dir_DIO_4 )
00242       cmd_pin_output_states_pre_ |= PIN_OUTPUT_DIRECTION_DIO_4;
00243     else
00244       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_4;
00245 
00246     if( config.pin_output_state_post_dir_DIO_4 )
00247       cmd_pin_output_states_post_ |= PIN_OUTPUT_DIRECTION_DIO_4;
00248     else
00249       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_4;
00250 
00251 
00252     if( config.pin_output_state_pre_DIO_5 )
00253       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_5;
00254     else
00255       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_5;
00256 
00257     if( config.pin_output_state_post_DIO_5 )
00258       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_5;
00259     else
00260       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_5;
00261 
00262     if( config.pin_output_state_pre_dir_DIO_5 )
00263       cmd_pin_output_states_pre_ |= PIN_OUTPUT_DIRECTION_DIO_5;
00264     else
00265       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_5;
00266 
00267     if( config.pin_output_state_post_dir_DIO_5 )
00268       cmd_pin_output_states_post_ |= PIN_OUTPUT_DIRECTION_DIO_5;
00269     else
00270       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_DIRECTION_DIO_5;
00271 
00272 
00273     if( config.pin_output_state_pre_CS_0 )
00274       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_0;
00275     else
00276       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_0;
00277 
00278     if( config.pin_output_state_post_CS_0 )
00279       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_0;
00280     else
00281       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_0;
00282 
00283 
00284     if( config.pin_output_state_pre_CS_1 )
00285       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_1;
00286     else
00287       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_1;
00288 
00289     if( config.pin_output_state_post_CS_1 )
00290       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_1;
00291     else
00292       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_1;
00293 
00294 
00295     if( config.pin_output_state_pre_CS_2 )
00296       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_2;
00297     else
00298       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_2;
00299 
00300     if( config.pin_output_state_post_CS_2 )
00301       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_2;
00302     else
00303       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_2;
00304 
00305 
00306     if( config.pin_output_state_pre_CS_3 )
00307       cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_3;
00308     else
00309       cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_3;
00310 
00311     if( config.pin_output_state_post_CS_3 )
00312       cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_3;
00313     else
00314       cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_3;
00315 
00316   }
00317 
00318 }
00319 
00320 /* For the emacs weenies in the crowd.
00321 Local Variables:
00322    c-basic-offset: 2
00323 End:
00324 */