sr_tcat.cpp
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 
00024 #include <sr_ronex_drivers/sr_tcat.hpp>
00025 
00026 #include <sstream>
00027 #include <iomanip>
00028 #include <boost/foreach.hpp>
00029 #include <math.h>
00030 #include <string>
00031 
00032 #include "sr_ronex_drivers/ronex_utils.hpp"
00033 
00034 PLUGINLIB_EXPORT_CLASS(SrTCAT, EthercatDevice);
00035 
00036 const std::string SrTCAT::product_alias_ = "tcat";
00037 
00038 SrTCAT::SrTCAT() :
00039   node_("~"), previous_sequence_number_(0)
00040 {}
00041 
00042 SrTCAT::~SrTCAT()
00043 {
00044   // remove parameters from server
00045   std::stringstream param_path;
00046   param_path << "/ronex/devices/" << parameter_id_;
00047   ros::param::del(param_path.str());
00048 }
00049 
00050 void SrTCAT::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00051 {
00052   sh_ = sh;
00053   serial_number_ = ronex::get_serial_number(sh);
00054 
00055   // get the alias from the parameter server if it exists
00056   std::string path_to_alias, alias;
00057   path_to_alias = "/ronex/mapping/" + serial_number_;
00058   if ( ros::param::get(path_to_alias, alias))
00059   {
00060     ronex_id_ = alias;
00061   }
00062   else
00063   {
00064     // no alias found, using the serial number directly.
00065     ronex_id_ = serial_number_;
00066   }
00067 
00068   device_name_ = ronex::build_name(product_alias_, ronex_id_);
00069 
00070   command_base_  = start_address;
00071   command_size_  = COMMAND_ARRAY_SIZE_BYTES;
00072 
00073   start_address += command_size_;
00074   status_base_   = start_address;
00075   status_size_   = STATUS_ARRAY_SIZE_BYTES;
00076 
00077   start_address += status_size_;
00078 
00079   // ETHERCAT_COMMAND_DATA
00080   //
00081   // This is for data going TO the board
00082   //
00083 
00084   if ( (PROTOCOL_TYPE) == (EC_BUFFERED) )
00085   {
00086     ROS_INFO("Using EC_BUFFERED");
00087   }
00088   else if ( (PROTOCOL_TYPE) == (EC_QUEUED) )
00089   {
00090     ROS_INFO("Using EC_QUEUED");
00091   }
00092 
00093   ROS_INFO("First FMMU (command) : Logical address: 0x%08X ; size: %3d bytes ; ET1200 address: 0x%08X", command_base_,
00094            command_size_, static_cast<int>(COMMAND_ADDRESS) );
00095   EC_FMMU *commandFMMU = new EC_FMMU( command_base_,            // Logical Start Address    (in ROS address space?)
00096                                       command_size_,
00097                                       0x00,                   // Logical Start Bit
00098                                       0x07,                   // Logical End Bit
00099                                       COMMAND_ADDRESS,        // Physical Start Address   (in ET1200 address space?)
00100                                       0x00,                   // Physical Start Bit
00101                                       false,                   // Read Enable
00102                                       true,                    // Write Enable
00103                                       true);                     // Channel Enable
00104 
00105 
00106 
00107   // WARNING!!!
00108   // We are leaving (command_size_ * 4) bytes in the physical memory of the device, but strictly we only need to
00109   // leave (command_size_ * 3). This change should be done in the firmware as well, otherwise it won't work.
00110   // This triple buffer is needed in the ethercat devices to work in EC_BUFFERED mode (in opposition to the other mode
00111   // EC_QUEUED, the so called mailbox mode)
00112 
00113   // ETHERCAT_STATUS_DATA
00114   //
00115   // This is for data coming FROM the board
00116   //
00117   ROS_INFO("Second FMMU (status) : Logical address: 0x%08X ; size: %3d bytes ; ET1200 address: 0x%08X", status_base_,
00118            status_size_, static_cast<int>(STATUS_ADDRESS) );
00119   EC_FMMU *statusFMMU = new EC_FMMU(  status_base_,
00120                                       status_size_,
00121                                       0x00,
00122                                       0x07,
00123                                       STATUS_ADDRESS,
00124                                       0x00,
00125                                       true,
00126                                       false,
00127                                       true);
00128 
00129 
00130   EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00131 
00132   (*fmmu)[0] = *commandFMMU;
00133   (*fmmu)[1] = *statusFMMU;
00134 
00135   sh->set_fmmu_config(fmmu);
00136 
00137   EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(2);
00138 
00139 // SyncMan takes the physical address
00140   (*pd)[0] = EC_SyncMan(COMMAND_ADDRESS,              command_size_,    PROTOCOL_TYPE, EC_WRITTEN_FROM_MASTER);
00141   (*pd)[1] = EC_SyncMan(STATUS_ADDRESS,               status_size_,     PROTOCOL_TYPE);
00142 
00143 
00144   (*pd)[0].ChannelEnable = true;
00145   (*pd)[0].ALEventEnable = true;
00146   (*pd)[0].WriteEvent    = true;
00147 
00148   (*pd)[1].ChannelEnable = true;
00149 
00150   sh->set_pd_config(pd);
00151 
00152   ROS_INFO("status_size_ : %d ; command_size_ : %d", status_size_, command_size_);
00153 
00154   ROS_INFO("Finished constructing the SrTCAT driver");
00155 }
00156 
00157 int SrTCAT::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00158 {
00159   ROS_INFO("Device #%02d: Product code: %u (%#010X) , Serial #: %u (%#010X)",
00160             sh_->get_ring_position(),
00161             sh_->get_product_code(),
00162             sh_->get_product_code(),
00163             sh_->get_serial(),
00164             sh_->get_serial());
00165 
00166   device_offset_ = sh_->get_ring_position();  // - hand_->getBridgeRingPosition();
00167 
00168   build_topics_();
00169 
00170   ROS_INFO_STREAM("Adding a "<< product_alias_ <<" RoNeX module to the hardware interface: " << device_name_);
00171   // Using the name of the ronex to prefix the state topic
00172 
00173   return 0;
00174 }
00175 
00176 void SrTCAT::packCommand(unsigned char *buffer, bool halt, bool reset)
00177 {
00178   RONEX_COMMAND_02000003* command = reinterpret_cast<RONEX_COMMAND_02000003*>(buffer);
00179 
00180   command->command_type = RONEX_COMMAND_02000003_COMMAND_TYPE_NORMAL;
00181 }
00182 
00183 bool SrTCAT::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00184 {
00185   RONEX_STATUS_02000003* status_data = reinterpret_cast<RONEX_STATUS_02000003 *>(this_buffer+  command_size_);
00186   u_int64_t timestamp_64;
00187 
00188   // Checking that the received command type matches RONEX_COMMAND_02000003_COMMAND_TYPE_NORMAL
00189   // (the one that was used in the command). The ronex firmware will answer with whatever command type we send.
00190   // The purpose of this is to filter those status_data structures that come filled with zeros due to the jitter
00191   // in the realtime loop. The jitter causes that the host tries to read the status when the microcontroller in the
00192   // ronex module has not finished writing it to memory yet.
00193   ROS_DEBUG_STREAM("-----\nNEW unpack");
00194   ROS_DEBUG_STREAM("   command type:    " << status_data->command_type <<
00195                            " ("<< RONEX_COMMAND_02000003_COMMAND_TYPE_NORMAL <<")");
00196   ROS_DEBUG_STREAM("   receiver number: " << status_data->receiver_number);
00197   ROS_DEBUG_STREAM("   seq number:      " << status_data->sequence_number << " (" << previous_sequence_number_ << ")");
00198 
00199   if ( status_data->command_type == RONEX_COMMAND_02000003_COMMAND_TYPE_NORMAL)
00200   {
00201     // ignore if receiver_number = -1 (data is not filled in)
00202     if ( status_data->receiver_number != -1 )
00203     {
00204       state_msg_.received_data[status_data->receiver_number].data_received = true;
00205 
00206       state_msg_.sequence_number = status_data->sequence_number;
00207       // fill in the state message with the new data.
00208       state_msg_.received_data[status_data->receiver_number].reserved.resize(NUM_RESERVED_WORDS);
00209       for (size_t i = 0 ; i < NUM_RESERVED_WORDS; ++i)
00210         state_msg_.received_data[status_data->receiver_number].reserved[i] = status_data->receiver_data.reserved[i];
00211 
00212       state_msg_.received_data[status_data->receiver_number].impulse_response.resize(IMPULSE_RESPONSE_SIZE);
00213       for (size_t i = 0 ; i < IMPULSE_RESPONSE_SIZE; ++i)
00214       {
00215         state_msg_.received_data[status_data->receiver_number].impulse_response[i].real =
00216                 status_data->receiver_data.impulse_response[i].real;
00217         state_msg_.received_data[status_data->receiver_number].impulse_response[i].imaginary =
00218                 status_data->receiver_data.impulse_response[i].imaginary;
00219       }
00220 
00221       state_msg_.received_data[status_data->receiver_number].first_sample_number =
00222               status_data->receiver_data.first_sample_number;
00223 
00224       state_msg_.received_data[status_data->receiver_number].payload.resize(PAYLOAD_MAX_SIZE);
00225       for (size_t i = 0 ; i < PAYLOAD_MAX_SIZE; ++i)
00226         state_msg_.received_data[status_data->receiver_number].payload[i] = status_data->receiver_data.payload[i];
00227 
00228       state_msg_.received_data[status_data->receiver_number].rx_frame_information =
00229               status_data->receiver_data.rx_frame_information;
00230       state_msg_.received_data[status_data->receiver_number].std_noise = status_data->receiver_data.std_noise;
00231       state_msg_.received_data[status_data->receiver_number].flags = status_data->receiver_data.flags;
00232       state_msg_.received_data[status_data->receiver_number].FPI =
00233               FPI_FIXED_POINT_TO_FLOAT(status_data->receiver_data.FPI);
00234 
00235       timestamp_64   = status_data->receiver_data.timestamp_H;
00236       timestamp_64 <<= 32;
00237       timestamp_64  += status_data->receiver_data.timestamp_L;
00238 
00239       state_msg_.received_data[status_data->receiver_number].timestamp_ns =
00240               static_cast<double>(timestamp_64) * (15.65/1000.0);
00241 
00242       ROS_DEBUG_STREAM("   timestamp:       " << state_msg_.received_data[status_data->receiver_number].timestamp_ns);
00243 
00244       // printf("0x%04x%08x = %f\n", status_data->receiver_data.timestamp_H, status_data->receiver_data.timestamp_L,
00245       // state_msg_.received_data[status_data->receiver_number].timestamp_ns);
00246       // state_msg_.received_data[status_data->receiver_number].timestamp_ns =
00247       // static_cast<double>(status_data->receiver_data.timestamp_L +
00248       // (static_cast<u_int64_t>(status_data->receiver_data.timestamp_H) << 32)*(15.65/1000.0));
00249     }
00250   }
00251 
00252   // publishing  if the sequence number is increased
00253   if ( status_data->sequence_number &&
00254       (status_data->sequence_number != previous_sequence_number_ )
00255      )
00256     {
00257       state_msg_.header.stamp = ros::Time::now();
00258 
00259       // publish the message
00260       if ( state_publisher_->trylock() )
00261       {
00262         state_publisher_->msg_ = state_msg_;
00263         state_publisher_->unlockAndPublish();
00264       }
00265 
00266       // reset the data received flags to false
00267       for (size_t i = 0; i < NUM_RECEIVERS; ++i)
00268         state_msg_.received_data[i].data_received = false;
00269 
00270       previous_sequence_number_ = status_data->sequence_number;
00271     }
00272 
00273   return true;
00274 }
00275 
00276 void SrTCAT::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00277 {
00278   d.name = device_name_;
00279   d.summary(d.OK, "OK");
00280   d.hardware_id = serial_number_;
00281 
00282   d.clear();
00283 }
00284 
00285 void SrTCAT::build_topics_()
00286 {
00287   // loading everything into the parameter server
00288   parameter_id_ = ronex::get_ronex_param_id("");
00289   std::stringstream param_path, tmp_param;
00290   param_path << "/ronex/devices/" << parameter_id_ << "/";
00291   tmp_param << ronex::get_product_code(sh_);
00292   ros::param::set(param_path.str() + "product_id", tmp_param.str());
00293   ros::param::set(param_path.str() + "product_name", product_alias_);
00294   ros::param::set(param_path.str() + "ronex_id", ronex_id_);
00295 
00296   // the device is stored using path as the key in the CustomHW map
00297   ros::param::set(param_path.str() + "path", device_name_);
00298   ros::param::set(param_path.str() + "serial", serial_number_);
00299 
00300   // Advertising the realtime state publisher
00301   state_publisher_.reset(new realtime_tools::RealtimePublisher<sr_ronex_msgs::TCATState>(node_, device_name_ +
00302           "/state", 1));
00303 }
00304 
00305 /* For the emacs weenies in the crowd.
00306    Local Variables:
00307    c-basic-offset: 2
00308    End:
00309 */


sr_ronex_drivers
Author(s): Ugo Cupcic, Toni Oliver, Mark Pitchless
autogenerated on Thu Jun 6 2019 21:22:00