sr_edc.cpp
Go to the documentation of this file.
1 
33 
35 
36 #include <algorithm>
37 #include <string>
38 #include <sstream>
39 #include <iomanip>
40 #include <boost/foreach.hpp>
41 #include <std_msgs/Int16.h>
42 #include <math.h>
43 #include <fcntl.h>
44 #include <stdio.h>
45 #include <pthread.h>
46 #include <bfd.h>
47 
48 #include <sr_utilities/sr_math_utils.hpp>
49 
50 using std::string;
51 using std::stringstream;
52 using std::vector;
53 
55 
56 extern "C"
57 {
59 }
60 
61 #include <boost/static_assert.hpp>
62 
63 namespace is_edc_command_32_bits
64 {
65 // check is the EDC_COMMAND is 32bits on the computer
66 // if not, fails
67  BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
68 } // namespace is_edc_command_32_bits
69 
70 const unsigned int SrEdc::max_retry = 20;
71 
72 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
73 
74 
75 #define check_for_pthread_mutex_init_error(x) switch (x) \
76  { \
77  case EAGAIN: \
78  ROS_ERROR("The system temporarily lacks the resources to create another mutex : %s:%d", __FILE__, __LINE__); \
79  exit(1); \
80  break; \
81  case EINVAL: \
82  ROS_ERROR("The value specified as attribute is invalid for mutex init : %s:%d", __FILE__, __LINE__); \
83  exit(1); \
84  break; \
85  case ENOMEM: \
86  ROS_ERROR("The process cannot allocate enough memory to create another mutex : %s:%d", __FILE__, __LINE__); \
87  exit(1); \
88  break; \
89  case 0: /* SUCCESS */ \
90  break; \
91  default: \
92  ROS_ERROR("unknown error value, is this POSIX system ? : %s:%d", __FILE__, __LINE__); \
93  exit(1); \
94  }
95 
96 #define unlock(x) switch ( pthread_mutex_unlock(x) ) \
97  { \
98  case EINVAL: \
99  ROS_ERROR("The value specified as a mutex is invalid : %s:%d", __FILE__, __LINE__); \
100  exit(1); \
101  break; \
102  case EPERM: \
103  ROS_ERROR("The current thread does not hold a lock on the mutex : %s:%d", __FILE__, __LINE__); \
104  exit(1); \
105  break; \
106  }
107 
108 #define check_for_trylock_error(x) if (x == EINVAL) \
109  { \
110  ROS_ERROR("mutex error %s:%d", __FILE__, __LINE__); \
111  exit(1); \
112  }
113 
121  : flashing(false),
122  can_message_sent(true),
123  can_packet_acked(true),
124  can_bus_(0),
125  counter_(0)
126 {
127  int res = 0;
129 
130  res = pthread_mutex_init(&producing, NULL);
132 }
133 
167 void SrEdc::construct(EtherCAT_SlaveHandler *sh, int &start_address, unsigned int ethercat_command_data_size,
168  unsigned int ethercat_status_data_size, unsigned int ethercat_can_bridge_data_size,
169  unsigned int ethercat_command_data_address, unsigned int ethercat_status_data_address,
170  unsigned int ethercat_can_bridge_data_command_address,
171  unsigned int ethercat_can_bridge_data_status_address)
172 {
173  sh_ = sh;
174 
175  // get the alias from the parameter server if it exists
176  std::string path_to_alias, alias;
177  path_to_alias = "/hand/mapping/" + boost::lexical_cast<std::string>(sh_->get_serial());
178  ROS_INFO_STREAM("Trying to read mapping for: " << path_to_alias);
179  if (ros::param::get(path_to_alias, alias))
180  {
181  device_id_ = alias;
182  }
183  else
184  {
185  // no alias found, using empty device_id_
186  // Using the serial number as we do in ronex is probably a worse option here.
187  device_id_ = "";
188  }
189  ros::NodeHandle nh_priv = ros::NodeHandle("~");
190  bool use_ns = true;
191  if (!nh_priv.getParam("use_ns", use_ns))
192  ROS_INFO_STREAM("use_ns not set for " << nh_priv.getNamespace());
193 
194  if (use_ns)
195  {
197  ROS_INFO_STREAM("Using namespace in sr_edc");
198  }
199  else
200  {
201  ROS_INFO_STREAM("Not using namespace in sr_edc");
203  }
205 
207 
208  // get the alias from the parameter server if it exists
209  std::string path_to_prefix, prefix;
210  path_to_prefix = "/hand/joint_prefix/" + boost::lexical_cast<std::string>(sh_->get_serial());
211  ROS_INFO_STREAM("Trying to read joint_prefix for: " << path_to_prefix);
212  if (ros::param::get(path_to_prefix, prefix))
213  {
214  device_joint_prefix_ = prefix;
215  }
216  else
217  {
218  // no prefix found, using empty prefix
220  }
221 
222  command_base_ = start_address;
223  command_size_ = ethercat_command_data_size + ethercat_can_bridge_data_size;
224 
225  start_address += command_size_;
226 
227  status_base_ = start_address;
228  status_size_ = ethercat_status_data_size + ethercat_can_bridge_data_size;
229 
230  start_address += status_size_;
231 
232  // ETHERCAT_COMMAND_DATA
233  //
234  // This is for data going TO the palm
235  //
236  ROS_INFO("First FMMU (command) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", command_base_,
237  command_size_,
238  static_cast<int> (ethercat_command_data_address));
239  EC_FMMU *commandFMMU = new EC_FMMU(command_base_, // Logical Start Address (in ROS address space?)
240  command_size_,
241  0x00, // Logical Start Bit
242  0x07, // Logical End Bit
243  ethercat_command_data_address, // Physical Start Address(in ET1200 address space?)
244  0x00, // Physical Start Bit
245  false, // Read Enable
246  true, // Write Enable
247  true); // Channel Enable
248 
249 
250 
251 
252  // ETHERCAT_STATUS_DATA
253  //
254  // This is for data coming FROM the palm
255  //
256  ROS_INFO("Second FMMU (status) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", status_base_,
257  status_size_,
258  static_cast<int> (ethercat_status_data_address));
259  EC_FMMU *statusFMMU = new EC_FMMU(status_base_,
260  status_size_,
261  0x00,
262  0x07,
263  ethercat_status_data_address,
264  0x00,
265  true,
266  false,
267  true);
268 
269 
270  EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
271 
272  (*fmmu)[0] = *commandFMMU;
273  (*fmmu)[1] = *statusFMMU;
274 
275  sh->set_fmmu_config(fmmu);
276 
277  EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(4);
278 
279  (*pd)[0] = EC_SyncMan(ethercat_command_data_address, ethercat_command_data_size, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
280  (*pd)[1] = EC_SyncMan(ethercat_can_bridge_data_command_address, ethercat_can_bridge_data_size, EC_QUEUED,
281  EC_WRITTEN_FROM_MASTER);
282  (*pd)[2] = EC_SyncMan(ethercat_status_data_address, ethercat_status_data_size, EC_QUEUED);
283  (*pd)[3] = EC_SyncMan(ethercat_can_bridge_data_status_address, ethercat_can_bridge_data_size, EC_QUEUED);
284 
285  status_size_ = ethercat_status_data_size + ethercat_can_bridge_data_size;
286 
287  (*pd)[0].ChannelEnable = true;
288  (*pd)[0].ALEventEnable = true;
289  (*pd)[0].WriteEvent = true;
290 
291  (*pd)[1].ChannelEnable = true;
292  (*pd)[1].ALEventEnable = true;
293  (*pd)[1].WriteEvent = true;
294 
295  (*pd)[2].ChannelEnable = true;
296  (*pd)[3].ChannelEnable = true;
297 
298  sh->set_pd_config(pd);
299 
300  ROS_INFO("status_size_ : %d ; command_size_ : %d", status_size_, command_size_);
301 }
302 
309 {
310  unsigned char cmd_sent;
311  unsigned int wait_time;
312  bool timedout = true;
313  unsigned int timeout;
314  int err;
315 
316  while (timedout)
317  {
318  ROS_INFO("Erasing FLASH");
319  // First we send the erase command
320  cmd_sent = 0;
321  while (!cmd_sent)
322  {
323  if (!(err = pthread_mutex_trylock(&producing)))
324  {
325  can_message_.message_length = 1;
326  can_message_.can_bus = can_bus_;
327  can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | ERASE_FLASH_COMMAND;
328  cmd_sent = 1;
329  unlock(&producing);
330  }
331  else
332  {
334  }
335  }
336  wait_time = 0;
337  timeout = 3000;
338  can_message_sent = false;
339  can_packet_acked = false;
340  timedout = false;
341  while (!can_packet_acked)
342  {
343  usleep(1000);
344  if (wait_time > timeout)
345  {
346  timedout = true;
347  break;
348  }
349  wait_time++;
350  }
351 
352  if (timedout)
353  {
354  ROS_ERROR("ERASE command timedout, resending it !");
355  }
356  };
357 }
358 
375 bool SrEdc::read_flash(unsigned int offset, unsigned int baddr)
376 {
377  unsigned int cmd_sent;
378  int err;
379  unsigned int wait_time;
380  bool timedout;
381  unsigned int timeout;
382  cmd_sent = 0;
383  while (!cmd_sent)
384  {
385  if (!(err = pthread_mutex_trylock(&producing)))
386  {
387  ROS_DEBUG("Sending READ data ... position : %03x", pos);
388  can_message_.can_bus = can_bus_;
389  can_message_.message_length = 3;
390  can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | READ_FLASH_COMMAND;
391  can_message_.message_data[2] = (offset + baddr) >> 16;
392  can_message_.message_data[1] = (offset + baddr) >> 8; // User application start address is 0x4C0
393  can_message_.message_data[0] = offset + baddr;
394  cmd_sent = 1;
395  unlock(&producing);
396  }
397  else
398  {
400  }
401  }
402  timedout = false;
403  wait_time = 0;
404  timeout = 100;
405  can_message_sent = false;
406  can_packet_acked = false;
407  while (!can_packet_acked)
408  {
409  usleep(1000);
410  if (wait_time > timeout)
411  {
412  timedout = true;
413  break;
414  }
415  wait_time++;
416  }
417  return timedout;
418 }
419 
458 bool SrEdc::simple_motor_flasher(sr_robot_msgs::SimpleMotorFlasher::Request &req,
459  sr_robot_msgs::SimpleMotorFlasher::Response &res)
460 {
461  bfd *fd;
462  unsigned int base_addr;
463  unsigned int smallest_start_address = 0x7fff;
464  unsigned int biggest_end_address = 0;
465  unsigned int total_size = 0;
466  bool timedout = true;
467 
469 
470  binary_content = NULL;
471  flashing = true;
472 
473  ROS_INFO("Flashing the motor");
474 
475  // Initialize the bfd library: "This routine must be called before any other BFD
476  // function to initialize magical internal data structures."
477  bfd_init();
478 
479  // Open the requested firmware object file
480  fd = bfd_openr(req.firmware.c_str(), NULL);
481  if (fd == NULL)
482  {
483  ROS_ERROR("error opening the file %s", get_filename(req.firmware).c_str());
484  res.value = res.FAIL;
485  flashing = false;
486  return false;
487  }
488 
489  // Check that bfd recognises the file as a correctly formatted object file
490  if (!bfd_check_format(fd, bfd_object))
491  {
492  if (bfd_get_error() != bfd_error_file_ambiguously_recognized)
493  {
494  ROS_ERROR("Incompatible format");
495  res.value = res.FAIL;
496  flashing = false;
497  return false;
498  }
499  }
500 
501  ROS_INFO("firmware %s's format is : %s.", get_filename(req.firmware).c_str(), fd->xvec->name);
502 
503  // @todo Check if it's necessary to send this dummy packet before the magic packet
504  ROS_DEBUG("Sending dummy packet");
505  send_CAN_msg(can_bus_, 0, 0, NULL, 1, &timedout);
506 
507  ROS_INFO_STREAM("Switching motor " << motor_being_flashed << " on CAN bus " << can_bus_ << " into bootloader mode");
508  // Send the magic packet that will force the microcontroller to go into bootloader mode
509  int8u magic_packet[] = {0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA};
510  send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | 0b1010, 8, magic_packet, 100, &timedout);
511 
512  // Send a second magic packet if the first one wasn't acknowledged
513  if (timedout)
514  {
515  ROS_WARN("First magic CAN packet timedout");
516  ROS_WARN("Sending another magic CAN packet to put the motor in bootloading mode");
517  send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | 0b1010, 8, magic_packet, 100, &timedout);
518 
519  if (timedout)
520  {
521  ROS_ERROR("None of the magic packets were ACKed, didn't bootload the motor.");
522  res.value = res.FAIL;
523  flashing = false;
524  return false;
525  }
526  }
527 
528  // Erase the PIC18 microcontroller flash memory
529  erase_flash();
530 
531  sleep(1);
532 
533  // Look for the start and end address of every section in the hex file,
534  // to detect the lowest and highest address of the data we need to write in the PIC's flash.
535  find_address_range(fd, &smallest_start_address, &biggest_end_address);
536 
537  // Calculate the size of the chunk of data to be flashed
538  total_size = biggest_end_address - smallest_start_address;
539  base_addr = smallest_start_address;
540 
541  // Allocate the memory space to store the data to be flashed
542  // This could be done with new bfd_byte[total_size+8] and delete() instead of malloc()
543  // and free() but will stay this way for the moment
544  binary_content = reinterpret_cast<bfd_byte *>(malloc(total_size + 8));
545  if (binary_content == NULL)
546  {
547  ROS_ERROR("Error allocating memory for binary_content");
548  res.value = res.FAIL;
549  flashing = false;
550  return false;
551  }
552 
553  // Set all the bytes in the binary_content to 0xFF initially (i.e. before reading the content from the hex file)
554  // This way we make sure that any byte in the region between smallest_start_address and biggest_end_address
555  // that is not included in any section of the hex file, will be written with a 0xFF value,
556  // which is the default in the PIC
557  memset(binary_content, 0xFF, total_size + 8);
558 
559  // The content of the firmware is read from the .hex file pointed by fd, to a memory region pointed by binary_content
560  if (!read_content_from_object_file(fd, binary_content, base_addr))
561  {
562  ROS_ERROR("something went wrong while parsing %s.", get_filename(req.firmware).c_str());
563  res.value = res.FAIL;
564  free(binary_content);
565  flashing = false;
566  return false;
567  }
568 
569  // We do not need the file anymore
570  bfd_close(fd);
571 
572  // The firmware is actually written to the flash memory of the PIC18
573  if (!write_flash_data(base_addr, total_size))
574  {
575  res.value = res.FAIL;
576  free(binary_content);
577  flashing = false;
578  return false;
579  }
580 
581 
582  ROS_INFO("Verifying");
583  // Now we have to read back the flash content
584  if (!read_back_and_check_flash(base_addr, total_size))
585  {
586  res.value = res.FAIL;
587  free(binary_content);
588  flashing = false;
589  return false;
590  }
591 
592 
593  free(binary_content);
594 
595  ROS_INFO("Resetting microcontroller.");
596  // Then we send the RESET command to PIC18F
597  timedout = true;
598  while (timedout)
599  {
600  send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | RESET_COMMAND, 0, NULL, 1000, &timedout);
601  };
602 
603  flashing = false;
604 
605  ROS_INFO("Flashing done");
606 
607  res.value = res.SUCCESS;
608 
609  // Reinitialize motor boards or valve controller boards information
611 
612  return true;
613 }
614 
615 void SrEdc::build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message)
616 {
617  int res;
618 
620  {
621  if (!(res = pthread_mutex_trylock(&producing)))
622  {
623  ROS_DEBUG_STREAM("Ethercat bridge data size: " << ETHERCAT_CAN_BRIDGE_DATA_SIZE);
624 
625  ROS_DEBUG("We're sending a CAN message for flashing.");
626  memcpy(message, &can_message_, sizeof(can_message_));
627  can_message_sent = true;
628 
629  ROS_DEBUG(
630  "Sending : SID : 0x%04X ; bus : 0x%02X ; length : 0x%02X ;"
631  " data : 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X",
632  message->message_id,
633  message->can_bus,
634  message->message_length,
635  message->message_data[0],
636  message->message_data[1],
637  message->message_data[2],
638  message->message_data[3],
639  message->message_data[4],
640  message->message_data[5],
641  message->message_data[6],
642  message->message_data[7]);
643 
644  unlock(&producing);
645  }
646  else
647  {
648  ROS_ERROR("Mutex is locked, we don't send any CAN message !");
650  }
651  }
652  else
653  {
654  message->can_bus = can_bus_;
655  message->message_id = 0x00;
656  message->message_length = 0;
657  }
658 }
659 
668 bool SrEdc::can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA *packet)
669 {
670  int i;
671 
672  if (packet->message_id == 0)
673  {
674  ROS_DEBUG("ID is zero");
675  return false;
676  }
677 
678  ROS_DEBUG("ack sid : %04X", packet->message_id);
679 
680  // Is this a reply to a READ request?
681  if ((packet->message_id & 0b0000011111111111) == (0x0600 | (motor_being_flashed << 5) | 0x10 | READ_FLASH_COMMAND))
682  {
683  ROS_DEBUG("READ reply %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", packet->message_data[0],
684  packet->message_data[1],
685  packet->message_data[2],
686  packet->message_data[3],
687  packet->message_data[4],
688  packet->message_data[5],
689  packet->message_data[6],
690  packet->message_data[7]);
691  ROS_DEBUG("Should be %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", binary_content[pos + 0],
692  binary_content[pos + 1],
693  binary_content[pos + 2],
694  binary_content[pos + 3],
695  binary_content[pos + 4],
696  binary_content[pos + 5],
697  binary_content[pos + 6],
698  binary_content[pos + 7]);
699 
700  if (!memcmp(packet->message_data, binary_content + pos, 8))
701  {
702  ROS_DEBUG("data is good");
703  return true;
704  }
705  else
706  {
707  ROS_DEBUG("data is bad");
708  return false;
709  }
710  }
711 
712  if (packet->message_length != can_message_.message_length)
713  {
714  ROS_DEBUG("Length is bad: %d", packet->message_length);
715  return false;
716  }
717 
718  ROS_DEBUG("Length is OK");
719 
720  for (i = 0; i < packet->message_length; ++i)
721  {
722  ROS_DEBUG("packet sent, data[%d] : %02X ; ack, data[%d] : %02X", i, can_message_.message_data[i], i,
723  packet->message_data[i]);
724  if (packet->message_data[i] != can_message_.message_data[i])
725  {
726  return false;
727  }
728  }
729  ROS_DEBUG("Data is OK");
730 
731  if (!(0x0010 & packet->message_id))
732  {
733  return false;
734  }
735 
736  ROS_DEBUG("This is an ACK");
737 
738  if ((packet->message_id & 0b0000000111101111) != (can_message_.message_id & 0b0000000111101111))
739  {
740  ROS_WARN_STREAM("Bad packet id: " << packet->message_id);
741  return false;
742  }
743 
744  ROS_DEBUG("SID is OK");
745 
746  ROS_DEBUG("Everything is OK, this is our ACK !");
747  return true;
748 }
749 
750 void SrEdc::send_CAN_msg(int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout)
751 {
752  int err;
753  unsigned char cmd_sent;
754  int wait_time;
755 
756  cmd_sent = 0;
757  while (!cmd_sent)
758  {
759  if (!(err = pthread_mutex_trylock(&producing)))
760  {
761  can_message_.message_length = msg_length;
762  can_message_.can_bus = can_bus;
763  can_message_.message_id = msg_id;
764 
765  if (msg_data != NULL)
766  {
767  for (unsigned int i = 0; i < msg_length; i++)
768  {
769  can_message_.message_data[i] = msg_data[i];
770  }
771  }
772 
773  cmd_sent = 1;
774  unlock(&producing);
775  }
776  else
777  {
779  }
780  }
781  wait_time = 0;
782  *timedout = false;
783  can_message_sent = false;
784  can_packet_acked = false;
785  while (!can_packet_acked)
786  {
787  usleep(1000); // 1 ms
788  wait_time++;
789  if (wait_time > timeout)
790  {
791  *timedout = true;
792  break;
793  }
794  }
795 }
796 
797 bool SrEdc::read_back_and_check_flash(unsigned int baddr, unsigned int total_size)
798 {
799  // The actual comparison between the content read from the flash and the content read from
800  // the hex file is carried out in the can_data_is_ack() function.
801  // read_flash(...) will return timedout = true if the 8 byte content read from the flash doesn't
802  // match the 8 bytes from the hex file
803  // BE CAREFUL with the pos "global" field, because it's being used inside can_data_is_ack()
804  // function to check if the response of the READ_FLASH_COMMAND is correct
805  pos = 0;
806  unsigned int retry;
807  while (pos < total_size)
808  {
809  bool timedout = true;
810 
811  retry = 0;
812  while (timedout)
813  {
814  timedout = read_flash(pos, baddr);
815  if (!timedout)
816  {
817  pos += 8;
818  }
819  retry++;
820  if (retry > max_retry)
821  {
822  ROS_ERROR("Too much retry for READ back, try flashing again");
823  return false;
824  }
825  };
826  }
827  return true;
828 }
829 
830 void SrEdc::find_address_range(bfd *fd, unsigned int *smallest_start_address, unsigned int *biggest_end_address)
831 {
832  asection *s;
833  unsigned int section_size = 0;
834  unsigned int section_addr = 0;
835 
836  // Look for the start and end address of every section in the hex file,
837  // to detect the lowest and highest address of the data we need to write in the PIC's flash.
838  // The sections starting at an address higher than 0x7fff will be ignored as they are not proper
839  // "code memory" firmware
840  // (they can contain the CONFIG bits of the microcontroller, which we don't want to write here)
841  // To understand the structure (sections) of the object file containing the firmware (usually a .hex) the following
842  // commands can be useful:
843  // \code objdump -x simplemotor.hex \endcode
844  // \code objdump -s simplemotor.hex \endcode
845  for (s = fd->sections; s; s = s->next)
846  {
847  // Only the sections with the LOAD flag on will be considered
848  if (bfd_get_section_flags(fd, s) & (SEC_LOAD))
849  {
850  // Only the sections with the same VMA (virtual memory address) and LMA (load MA) will be considered
851  // http://www.delorie.com/gnu/docs/binutils/ld_7.html
852  if (bfd_section_lma(fd, s) == bfd_section_vma(fd, s))
853  {
854  section_addr = (unsigned int) bfd_section_lma(fd, s);
855  if (section_addr >= 0x7fff)
856  {
857  continue;
858  }
859  section_size = (unsigned int) bfd_section_size(fd, s);
860  *smallest_start_address = std::min(section_addr, *smallest_start_address);
861  *biggest_end_address = std::max(*biggest_end_address, section_addr + section_size);
862  }
863  }
864  }
865 }
866 
867 bool SrEdc::read_content_from_object_file(bfd *fd, bfd_byte *content, unsigned int base_addr)
868 {
869  asection *s;
870  unsigned int section_size = 0;
871  unsigned int section_addr = 0;
872 
873  for (s = fd->sections; s; s = s->next)
874  {
875  // Only the sections with the LOAD flag on will be considered
876  if (bfd_get_section_flags(fd, s) & (SEC_LOAD))
877  {
878  // Only the sections with the same VMA (virtual memory address) and LMA (load MA) will be considered
879  // http://www.delorie.com/gnu/docs/binutils/ld_7.html
880  if (bfd_section_lma(fd, s) == bfd_section_vma(fd, s))
881  {
882  section_addr = (unsigned int) bfd_section_lma(fd, s);
883  // The sections starting at an address higher than 0x7fff will be ignored as they are
884  // not proper "code memory" firmware
885  // (they can contain the CONFIG bits of the microcontroller, which we don't want to write here)
886  if (section_addr >= 0x7fff)
887  {
888  continue;
889  }
890  section_size = (unsigned int) bfd_section_size(fd, s);
891  bfd_get_section_contents(fd, s, content + (section_addr - base_addr), 0, section_size);
892  }
893  else
894  {
895  return false;
896  }
897  }
898  else
899  {
900  return false;
901  }
902  }
903  return true;
904 }
905 
906 bool SrEdc::write_flash_data(unsigned int base_addr, unsigned int total_size)
907 {
908  int err;
909  unsigned char cmd_sent;
910  int wait_time, timeout;
911 
912  pos = 0;
913  unsigned int packet = 0;
914  ROS_INFO("Sending the firmware data");
915  while (pos < ((total_size % 32) == 0 ? total_size : (total_size + 32 - (total_size % 32))))
916  {
917  bool timedout = true;
918 
919  // For every WRITE_FLASH_ADDRESS_COMMAND we write 32 bytes of data to flash
920  // and this is done by sending 4 WRITE_FLASH_DATA_COMMAND packets, every one containing
921  // 8 bytes of data to be written
922  if ((pos % 32) == 0)
923  {
924  packet = 0;
925  while (timedout)
926  {
927  cmd_sent = 0;
928  while (!cmd_sent)
929  {
930  if (!(err = pthread_mutex_trylock(&producing)))
931  {
932  can_message_.message_length = 3;
933  can_message_.can_bus = can_bus_;
934  can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | WRITE_FLASH_ADDRESS_COMMAND;
935  can_message_.message_data[2] = (base_addr + pos) >> 16;
936  can_message_.message_data[1] = (base_addr + pos) >> 8; // User application start address is 0x4C0
937  can_message_.message_data[0] = base_addr + pos;
938  ROS_DEBUG("Sending write address to motor %d : 0x%02X%02X%02X", motor_being_flashed,
939  can_message_.message_data[2], can_message_.message_data[1], can_message_.message_data[0]);
940  cmd_sent = 1;
941  unlock(&producing);
942  }
943  else
944  {
946  }
947  }
948  wait_time = 0;
949  timedout = false;
950  timeout = 100;
951  can_message_sent = false;
952  can_packet_acked = false;
953  while (!can_packet_acked)
954  {
955  usleep(1000);
956  if (wait_time > timeout)
957  {
958  timedout = true;
959  break;
960  }
961  wait_time++;
962  }
963  if (timedout)
964  {
965  ROS_ERROR("WRITE ADDRESS timedout ");
966  }
967  };
968  }
969  cmd_sent = 0;
970  while (!cmd_sent)
971  {
972  if (!(err = pthread_mutex_trylock(&producing)))
973  {
974  ROS_DEBUG("Sending data ... position == %d", pos);
975  can_message_.message_length = 8;
976  can_message_.can_bus = can_bus_;
977  can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | WRITE_FLASH_DATA_COMMAND;
978  bzero(can_message_.message_data, 8);
979  for (unsigned char j = 0; j < 8; ++j)
980  {
981  can_message_.message_data[j] = (pos > total_size) ? 0xFF : *(binary_content + pos + j);
982  }
983 
984  pos += 8;
985  cmd_sent = 1;
986  unlock(&producing);
987  }
988  else
989  {
991  }
992  }
993  packet++;
994  timedout = false;
995  wait_time = 0;
996  timeout = 100;
997  can_message_sent = false;
998  can_packet_acked = false;
999  while (!can_packet_acked)
1000  {
1001  usleep(1000);
1002  if (wait_time > timeout)
1003  {
1004  timedout = true;
1005  break;
1006  }
1007  wait_time++;
1008  }
1009  if (timedout)
1010  {
1011  ROS_ERROR("A WRITE data packet has been lost at pos=%u, resending the 32 bytes block at pos=%u !", pos,
1012  (pos - packet * 8));
1013  pos -= packet * 8;
1014  }
1015  }
1016  return true;
1017 }
1018 
1019 
1020 /* For the emacs weenies in the crowd.
1021  Local Variables:
1022  c-basic-offset: 2
1023  End:
1024 */
unsigned short int16u
unsigned int motor_being_flashed
Definition: sr_edc.h:130
ros::NodeHandle nh_tilde_
Definition: sr_edc.h:87
bfd_byte * binary_content
Definition: sr_edc.h:128
bool simple_motor_flasher(sr_robot_msgs::SimpleMotorFlasher::Request &req, sr_robot_msgs::SimpleMotorFlasher::Response &res)
ROS Service that flashes a new firmware into a SimpleMotor board.
Definition: sr_edc.cpp:458
READ_FLASH_COMMAND
XmlRpcServer s
static const unsigned int max_retry
Definition: sr_edc.h:116
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
int command_base_
Definition: sr0x.h:67
#define ROS_WARN(...)
#define check_for_pthread_mutex_init_error(x)
Definition: sr_edc.cpp:75
bool read_flash(unsigned int offset, unsigned int baddr)
Function that reads back 8 bytes from PIC18F program memory.
Definition: sr_edc.cpp:375
virtual void reinitialize_boards()=0
This function will call the reinitialization function for the boards attached to the CAN bus...
BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND)==4)
pthread_mutex_t producing
Definition: sr_edc.h:122
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)
Construct function, run at startup to set SyncManagers and FMMUs.
Definition: sr_edc.cpp:167
unsigned char int8u
bool can_packet_acked
Definition: sr_edc.h:96
ETHERCAT_CAN_BRIDGE_DATA can_message_
Definition: sr_edc.h:126
virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)=0
bool can_message_sent
Definition: sr_edc.h:127
ROSCPP_DECL bool get(const std::string &key, std::string &s)
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
bool write_flash_data(unsigned int base_addr, unsigned int total_size)
Definition: sr_edc.cpp:906
RESET_COMMAND
ros::NodeHandle nodehandle_
Definition: sr_edc.h:86
void send_CAN_msg(int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout)
Definition: sr_edc.cpp:750
void erase_flash()
Erase the PIC18F Flash memory.
Definition: sr_edc.cpp:308
void build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message)
Definition: sr_edc.cpp:615
SrEdc()
Constructor of the SrEdc driver.
Definition: sr_edc.cpp:120
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
#define unlock(x)
Definition: sr_edc.cpp:96
bool can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA *packet)
This function checks if the can packet in the unpackState() this_buffer is an ACK.
Definition: sr_edc.cpp:668
unsigned int pos
Definition: sr_edc.h:129
bool read_content_from_object_file(bfd *fd, bfd_byte *content, unsigned int base_addr)
Definition: sr_edc.cpp:867
#define ROS_INFO_STREAM(args)
const std::string & getNamespace() const
int can_bus_
Definition: sr_edc.h:134
#define check_for_trylock_error(x)
Definition: sr_edc.cpp:108
static std::string get_filename(std::string full_path)
Definition: sr_edc.h:202
std::string device_joint_prefix_
Definition: sr_edc.h:99
std::string device_id_
Definition: sr_edc.h:98
This is a parent class for the ROS drivers for any Shadow Robot EtherCAT Dual CAN Slave...
EDC_COMMAND
void find_address_range(bfd *fd, unsigned int *smallest_start_address, unsigned int *biggest_end_address)
Definition: sr_edc.cpp:830
WRITE_FLASH_DATA_COMMAND
ros::ServiceServer serviceServer
Definition: sr_edc.h:123
ERASE_FLASH_COMMAND
#define ROS_ERROR(...)
bool flashing
Definition: sr_edc.h:95
int status_base_
Definition: sr0x.h:68
WRITE_FLASH_ADDRESS_COMMAND
#define ETHERCAT_CAN_BRIDGE_DATA_SIZE
Definition: sr_edc.cpp:72
bool read_back_and_check_flash(unsigned int baddr, unsigned int total_size)
Definition: sr_edc.cpp:797
#define ROS_DEBUG(...)


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:53