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::RobotStateInterface* 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(
00044             node_.advertiseService<sr_ronex_msgs::SPI::Request, sr_ronex_msgs::SPI::Response>(service_path.str(),
00045                                                   boost::bind(&SPIPassthroughController::command_srv_cb,
00046                                                               this, _1, _2,  i)) );
00047   }
00048 
00049   dynamic_reconfigure_server_.reset(
00050           new dynamic_reconfigure::Server<sr_ronex_drivers::SPIConfig>(ros::NodeHandle(topic_prefix_)));
00051   function_cb_ = boost::bind(&SPIPassthroughController::dynamic_reconfigure_cb, this, _1, _2);
00052   dynamic_reconfigure_server_->setCallback(function_cb_);
00053 
00054   return true;
00055 }
00056 
00057 bool SPIPassthroughController::command_srv_cb(sr_ronex_msgs::SPI::Request &req,
00058                                               sr_ronex_msgs::SPI::Response &res,
00059                                               size_t spi_out_index)
00060 {
00061   // transmitting the bytes we received
00062   standard_commands_[spi_out_index].packet.num_bytes = static_cast<int8u>(req.data.size());
00063 
00064   ROS_DEBUG_STREAM("From passthrough: received "<< req.data.size() << "bytes.");
00065   for (size_t i = 0; i < req.data.size(); ++i)
00066   {
00067     try
00068     {
00069       standard_commands_[spi_out_index].packet.data_bytes[i] = static_cast<int8u>(req.data[i]);
00070     }
00071     catch(...)
00072     {
00073       ROS_ERROR_STREAM("Input[" << i << "]: " << req.data[i] << " could not be converted to int");
00074     }
00075   }
00076 
00077   // pushing to the command queue to be sent through etherCAT
00078   command_queue_[spi_out_index].push(standard_commands_[spi_out_index]);
00079 
00080   // wait for the response to be received
00081   bool not_received = true;
00082   while ( not_received )
00083   {
00084     // sleep roughly 1ms to wait for new etherCAT packet to be received.
00085     usleep(1000);
00086 
00087     // check if the status_queue has the same command and that the response has been received
00088     if (status_queue_[spi_out_index].size() > 0 &&
00089         std::equal(status_queue_[spi_out_index].front().first.packet.data_bytes,
00090         status_queue_[spi_out_index].front().first.packet.data_bytes +
00091         sizeof status_queue_[spi_out_index].front().first.packet.data_bytes /
00092         sizeof *status_queue_[spi_out_index].front().first.packet.data_bytes,
00093         standard_commands_[spi_out_index].packet.data_bytes) &&
00094         status_queue_[spi_out_index].front().second.received == true)
00095     {
00096       // found the status command corresponding to the command we sent
00097       // updating the response
00098       for (size_t j = 0; j < req.data.size(); ++j)
00099       {
00100         std::ostringstream hex;
00101         try
00102         {
00103           hex << static_cast<unsigned int>(status_queue_[spi_out_index].front().second.packet.data_bytes[j]);
00104         }
00105         catch(...)
00106         {
00107           ROS_ERROR_STREAM("Can't cast to uint.");
00108           hex << "bad_data";
00109         }
00110         res.data.push_back(hex.str());
00111       }
00112       not_received = false;
00113 
00114       // we used the status (sent it back to the user through the service
00115       // response -> set flag to pop status from the queue
00116       delete_status_[spi_out_index] = true;
00117       break;
00118     }
00119   }
00120   return true;
00121 }
00122 
00123 void SPIPassthroughController::dynamic_reconfigure_cb(sr_ronex_drivers::SPIConfig &config, uint32_t level)
00124 {
00125   spi_->command_->command_type = static_cast<int16u>(config.command_type);
00126 
00127   // setting up spi 0
00128   standard_commands_[0].packet.clock_divider = static_cast<int16u>(config.spi_0_clock_divider);
00129   standard_commands_[0].packet.SPI_config = 0;
00130   standard_commands_[0].packet.SPI_config |= static_cast<int16u>(config.spi_mode_0);
00131   standard_commands_[0].packet.SPI_config |= static_cast<int16u>(config.spi_0_input_trigger);
00132   standard_commands_[0].packet.SPI_config |= static_cast<int16u>(config.spi_0_mosi_somi);
00133   standard_commands_[0].packet.inter_byte_gap = static_cast<int16u>(config.spi_0_inter_byte_gap);
00134 
00135   // setting up spi 1
00136   standard_commands_[1].packet.clock_divider = static_cast<int16u>(config.spi_1_clock_divider);
00137   standard_commands_[1].packet.SPI_config = 0;
00138   standard_commands_[1].packet.SPI_config |= static_cast<int16u>(config.spi_mode_1);
00139   standard_commands_[1].packet.SPI_config |= static_cast<int16u>(config.spi_1_input_trigger);
00140   standard_commands_[1].packet.SPI_config |= static_cast<int16u>(config.spi_1_mosi_somi);
00141   standard_commands_[1].packet.inter_byte_gap = static_cast<int16u>(config.spi_1_inter_byte_gap);
00142 
00143   // setting up spi 2
00144   standard_commands_[2].packet.clock_divider = static_cast<int16u>(config.spi_2_clock_divider);
00145   standard_commands_[2].packet.SPI_config = 0;
00146   standard_commands_[2].packet.SPI_config |= static_cast<int16u>(config.spi_mode_2);
00147   standard_commands_[2].packet.SPI_config |= static_cast<int16u>(config.spi_2_input_trigger);
00148   standard_commands_[2].packet.SPI_config |= static_cast<int16u>(config.spi_2_mosi_somi);
00149   standard_commands_[2].packet.inter_byte_gap = static_cast<int16u>(config.spi_2_inter_byte_gap);
00150 
00151   // setting up spi 3
00152   standard_commands_[3].packet.clock_divider = static_cast<int16u>(config.spi_3_clock_divider);
00153   standard_commands_[3].packet.SPI_config = 0;
00154   standard_commands_[3].packet.SPI_config |= static_cast<int16u>(config.spi_mode_3);
00155   standard_commands_[3].packet.SPI_config |= static_cast<int16u>(config.spi_3_input_trigger);
00156   standard_commands_[3].packet.SPI_config |= static_cast<int16u>(config.spi_3_mosi_somi);
00157   standard_commands_[3].packet.inter_byte_gap = static_cast<int16u>(config.spi_3_inter_byte_gap);
00158 
00159 
00160   if ( config.pin_output_state_pre_DIO_0 )
00161     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_0;
00162   else
00163     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_0;
00164 
00165   if ( config.pin_output_state_post_DIO_0 )
00166     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_0;
00167   else
00168     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_0;
00169 
00170 
00171   if ( config.pin_output_state_pre_DIO_1 )
00172     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_1;
00173   else
00174     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_1;
00175 
00176   if ( config.pin_output_state_post_DIO_1 )
00177     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_1;
00178   else
00179     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_1;
00180 
00181 
00182   if ( config.pin_output_state_pre_DIO_2 )
00183     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_2;
00184   else
00185     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_2;
00186 
00187   if ( config.pin_output_state_post_DIO_2 )
00188     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_2;
00189   else
00190     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_2;
00191 
00192 
00193   if ( config.pin_output_state_pre_DIO_3 )
00194     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_3;
00195   else
00196     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_3;
00197 
00198   if ( config.pin_output_state_post_DIO_3 )
00199     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_3;
00200   else
00201     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_3;
00202 
00203 
00204   if ( config.pin_output_state_pre_DIO_4 )
00205     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_4;
00206   else
00207     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_4;
00208 
00209   if ( config.pin_output_state_post_DIO_4 )
00210     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_4;
00211   else
00212     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_4;
00213 
00214 
00215   if ( config.pin_output_state_pre_DIO_5 )
00216     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_DIO_5;
00217   else
00218     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_5;
00219 
00220   if ( config.pin_output_state_post_DIO_5 )
00221     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_DIO_5;
00222   else
00223     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_DIO_5;
00224 
00225 
00226   if ( config.pin_output_state_pre_CS_0 )
00227     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_0;
00228   else
00229     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_0;
00230 
00231   if ( config.pin_output_state_post_CS_0 )
00232     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_0;
00233   else
00234     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_0;
00235 
00236 
00237   if ( config.pin_output_state_pre_CS_1 )
00238     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_1;
00239   else
00240     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_1;
00241 
00242   if ( config.pin_output_state_post_CS_1 )
00243     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_1;
00244   else
00245     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_1;
00246 
00247 
00248   if ( config.pin_output_state_pre_CS_2 )
00249     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_2;
00250   else
00251     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_2;
00252 
00253   if ( config.pin_output_state_post_CS_2 )
00254     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_2;
00255   else
00256     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_2;
00257 
00258 
00259   if ( config.pin_output_state_pre_CS_3 )
00260     cmd_pin_output_states_pre_ |= PIN_OUTPUT_STATE_CS_3;
00261   else
00262     cmd_pin_output_states_pre_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_3;
00263 
00264   if ( config.pin_output_state_post_CS_3 )
00265     cmd_pin_output_states_post_ |= PIN_OUTPUT_STATE_CS_3;
00266   else
00267     cmd_pin_output_states_post_ &= 0xFFFF - PIN_OUTPUT_STATE_CS_3;
00268 }
00269 
00270 }  // namespace ronex
00271 
00272 /* For the emacs weenies in the crowd.
00273 Local Variables:
00274    c-basic-offset: 2
00275 End:
00276 */