sr_edc.h
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   // bool read_flash(unsigned int offset, unsigned char baddrl, unsigned char baddrh, unsigned char baddru);
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   // static const unsigned int        nb_sensors_const;
00117   static const unsigned int max_retry;
00118   // static const unsigned short int  max_iter_const;
00119   // static const unsigned short int  ros_pub_freq_const;
00120   // static const unsigned short int  device_pub_freq_const;
00121   // static const unsigned char       nb_publish_by_unpack_const;
00122   // std::string                      firmware_file_name;
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;  // buffer containing the binary content to be flashed
00130   unsigned int pos;  // position in binary_content buffer
00131   unsigned int motor_being_flashed;
00132 
00133 
00134   // We're using 2 can busses, so can_bus_ is 1 for motors 0 to 9 and 2 for motors 10 to 19
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 /* For the emacs weenies in the crowd.
00213 Local Variables:
00214    c-basic-offset: 2
00215 End:
00216 */
00217 
00218 
00219 #endif  // SR_EDC_ETHERCAT_DRIVERS_SR_EDC_H


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Mon Jul 1 2019 20:06:31