sr_edc.h

Go to the documentation of this file.

#ifndef SR_EDC_H
#define SR_EDC_H

#include <ethercat_hardware/ethercat_device.h>
#include <sr_edc_ethercat_drivers/sr0x.h>
#include <realtime_tools/realtime_publisher.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Float64MultiArray.h>
#include <sr_robot_msgs/SimpleMotorFlasher.h>
#include <pthread.h>
#include <bfd.h>
#include <boost/smart_ptr.hpp>
#include <map>
#include <boost/assign.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/find_iterator.hpp>

//#include <sr_robot_lib/sr_hand_lib.hpp>

#include <sr_robot_msgs/EthercatDebug.h>

#include <sr_external_dependencies/types_for_external.h>
extern "C"
{
  #include <sr_external_dependencies/external/common/ethercat_can_bridge_protocol.h>
  #include <sr_external_dependencies/external/common/common_edc_ethercat_protocol.h>
}


class SrEdc : public SR0X
{
public:
  SrEdc();
  virtual ~SrEdc();

  virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address);
  virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address, unsigned int ethercat_command_data_size, unsigned int ethercat_status_data_size, unsigned int ethercat_can_bridge_data_size,
                                                 unsigned int ethercat_command_data_address, unsigned int ethercat_status_data_address, unsigned int ethercat_can_bridge_data_command_address, unsigned int ethercat_can_bridge_data_status_address);
  virtual int  initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true);

  bool simple_motor_flasher(sr_robot_msgs::SimpleMotorFlasher::Request &req, sr_robot_msgs::SimpleMotorFlasher::Response &res);
  void build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message);
  bool can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA * packet);
  void erase_flash();
  //bool read_flash(unsigned int offset, unsigned char baddrl, unsigned char baddrh, unsigned char baddru);
  bool read_flash(unsigned int offset, unsigned int baddr);

protected:
  int                                                                  counter_;
  ros::NodeHandle                                                      nodehandle_;

  typedef realtime_tools::RealtimePublisher<std_msgs::Int16> rt_pub_int16_t;
  std::vector< boost::shared_ptr<rt_pub_int16_t> >   realtime_pub_;

  boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> > extra_analog_inputs_publisher;

  bool                             flashing;
  bool                             can_packet_acked;

  virtual void reinitialize_boards() = 0;

  virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id) = 0;
private:

  //static const unsigned int        nb_sensors_const;
  static const unsigned int        max_retry;
  //static const unsigned short int  max_iter_const;
  //static const unsigned short int  ros_pub_freq_const;
  //static const unsigned short int  device_pub_freq_const;
  //static const unsigned char       nb_publish_by_unpack_const;
  //std::string                      firmware_file_name;
  pthread_mutex_t                  producing;
  ros::ServiceServer               serviceServer;


  ETHERCAT_CAN_BRIDGE_DATA         can_message_;
  bool                            can_message_sent;
  bfd_byte                        *binary_content; // buffer containing the binary content to be flashed
  unsigned int                     pos; // position in binary_content buffer
  unsigned int                     motor_being_flashed;


  int can_bus_;

  void send_CAN_msg(int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout);

  bool read_back_and_check_flash(unsigned int baddr, unsigned int total_size);

  void find_address_range(bfd *fd, unsigned int *smallest_start_address, unsigned int *biggest_end_address);

  bool read_content_from_object_file(bfd *fd, bfd_byte *content, unsigned int base_addr);

  bool write_flash_data(unsigned int base_addr, unsigned int total_size);

  static inline std::string get_filename( std::string full_path )
  {
    std::vector<std::string> splitted_string;
    boost::split(splitted_string, full_path, boost::is_any_of("/"));
    return splitted_string.back();
  }

};


/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/


#endif /* SR_EDC_H */