Go to the documentation of this file.00001
00032 #ifndef SR_EDC_ETHERCAT_DRIVERS_SR_EDC_H
00033 #define SR_EDC_ETHERCAT_DRIVERS_SR_EDC_H
00034
00035 #include <ros_ethercat_hardware/ethercat_hardware.h>
00036 #include <sr_edc_ethercat_drivers/sr0x.h>
00037 #include <realtime_tools/realtime_publisher.h>
00038 #include <std_msgs/Int16.h>
00039 #include <std_msgs/Float64MultiArray.h>
00040 #include <sr_robot_msgs/SimpleMotorFlasher.h>
00041 #include <pthread.h>
00042 #include <bfd.h>
00043 #include <boost/smart_ptr.hpp>
00044 #include <map>
00045 #include <string>
00046 #include <vector>
00047 #include <boost/assign.hpp>
00048 #include <boost/algorithm/string.hpp>
00049 #include <boost/algorithm/string/find_iterator.hpp>
00050 #include <sr_robot_msgs/EthercatDebug.h>
00051
00052 #include <sr_external_dependencies/types_for_external.h>
00053
00054 extern "C"
00055 {
00056 #include <sr_external_dependencies/external/common/ethercat_can_bridge_protocol.h>
00057 #include <sr_external_dependencies/external/common/common_edc_ethercat_protocol.h>
00058 }
00059
00060
00061 class SrEdc :
00062 public SR0X
00063 {
00064 public:
00065 SrEdc();
00066
00067 virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address, unsigned int ethercat_command_data_size,
00068 unsigned int ethercat_status_data_size, unsigned int ethercat_can_bridge_data_size,
00069 unsigned int ethercat_command_data_address, unsigned int ethercat_status_data_address,
00070 unsigned int ethercat_can_bridge_data_command_address,
00071 unsigned int ethercat_can_bridge_data_status_address);
00072
00073 bool simple_motor_flasher(sr_robot_msgs::SimpleMotorFlasher::Request &req,
00074 sr_robot_msgs::SimpleMotorFlasher::Response &res);
00075
00076 void build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message);
00077
00078 bool can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA *packet);
00079
00080 void erase_flash();
00081
00082
00083 bool read_flash(unsigned int offset, unsigned int baddr);
00084
00085 protected:
00086 int counter_;
00087 ros::NodeHandle nodehandle_;
00088 ros::NodeHandle nh_tilde_;
00089
00090 typedef realtime_tools::RealtimePublisher<std_msgs::Int16> rt_pub_int16_t;
00091 std::vector<boost::shared_ptr<rt_pub_int16_t> > realtime_pub_;
00092
00094 boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> > extra_analog_inputs_publisher;
00095
00096 bool flashing;
00097 bool can_packet_acked;
00098
00099 std::string device_id_;
00100 std::string device_joint_prefix_;
00101
00103 virtual void reinitialize_boards() = 0;
00104
00113 virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id) = 0;
00114
00115 private:
00116
00117 static const unsigned int max_retry;
00118
00119
00120
00121
00122
00123 pthread_mutex_t producing;
00124 ros::ServiceServer serviceServer;
00125
00126
00127 ETHERCAT_CAN_BRIDGE_DATA can_message_;
00128 bool can_message_sent;
00129 bfd_byte *binary_content;
00130 unsigned int pos;
00131 unsigned int motor_being_flashed;
00132
00133
00134
00135 int can_bus_;
00136
00148 void send_CAN_msg(int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout);
00149
00158 bool read_back_and_check_flash(unsigned int baddr, unsigned int total_size);
00159
00173 void find_address_range(bfd *fd, unsigned int *smallest_start_address, unsigned int *biggest_end_address);
00174
00184 bool read_content_from_object_file(bfd *fd, bfd_byte *content, unsigned int base_addr);
00185
00194 bool write_flash_data(unsigned int base_addr, unsigned int total_size);
00195
00203 static inline std::string get_filename(std::string full_path)
00204 {
00205 std::vector<std::string> splitted_string;
00206 boost::split(splitted_string, full_path, boost::is_any_of("/"));
00207 return splitted_string.back();
00208 }
00209 };
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219 #endif // SR_EDC_ETHERCAT_DRIVERS_SR_EDC_H