00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
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
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
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
00080
00081
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_,
00096 command_size_,
00097 0x00,
00098 0x07,
00099 COMMAND_ADDRESS,
00100 0x00,
00101 false,
00102 true,
00103 true);
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
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
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();
00167
00168 build_topics_();
00169
00170 ROS_INFO_STREAM("Adding a "<< product_alias_ <<" RoNeX module to the hardware interface: " << device_name_);
00171
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
00189
00190
00191
00192
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
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
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
00245
00246
00247
00248
00249 }
00250 }
00251
00252
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
00260 if ( state_publisher_->trylock() )
00261 {
00262 state_publisher_->msg_ = state_msg_;
00263 state_publisher_->unlockAndPublish();
00264 }
00265
00266
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
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
00297 ros::param::set(param_path.str() + "path", device_name_);
00298 ros::param::set(param_path.str() + "serial", serial_number_);
00299
00300
00301 state_publisher_.reset(new realtime_tools::RealtimePublisher<sr_ronex_msgs::TCATState>(node_, device_name_ +
00302 "/state", 1));
00303 }
00304
00305
00306
00307
00308
00309