sr_edc.cpp
Go to the documentation of this file.
00001 
00033 #include <sr_edc_ethercat_drivers/sr_edc.h>
00034 
00035 #include <dll/ethercat_dll.h>
00036 #include <al/ethercat_AL.h>
00037 #include <dll/ethercat_device_addressed_telegram.h>
00038 #include <dll/ethercat_frame.h>
00039 #include <realtime_tools/realtime_publisher.h>
00040 
00041 #include <math.h>
00042 #include <sstream>
00043 #include <iomanip>
00044 #include <boost/foreach.hpp>
00045 #include <std_msgs/Int16.h>
00046 #include <math.h>
00047 #include <fcntl.h>
00048 #include <stdio.h>
00049 #include <pthread.h>
00050 #include <bfd.h>
00051 
00052 #include <sr_utilities/sr_math_utils.hpp>
00053 
00054 using namespace std;
00055 
00056 #include <sr_external_dependencies/types_for_external.h>
00057 extern "C"
00058 {
00059   #include <sr_external_dependencies/external/simplemotor-bootloader/bootloader.h>
00060 }
00061 
00062 #include <boost/static_assert.hpp>
00063 namespace is_edc_command_32_bits
00064 {
00065   //check is the EDC_COMMAND is 32bits on the computer
00066   //if not, fails
00067   BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
00068 } // namespace is_edc_command_32_bits
00069 
00070 const unsigned int       SrEdc::max_retry                  = 20;
00071 
00072 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
00073 
00074 
00075 
00076 #define check_for_pthread_mutex_init_error(x)   switch(x)               \
00077   {                                                                     \
00078   case EAGAIN:                                                          \
00079     ROS_ERROR("The system temporarily lacks the resources to create another mutex : %s:%d", __FILE__, __LINE__); \
00080     exit(1);                                                            \
00081     break;                                                              \
00082   case EINVAL:                                                          \
00083     ROS_ERROR("The value specified as attribute is invalid for mutex init : %s:%d", __FILE__, __LINE__); \
00084     exit(1);                                                            \
00085     break;                                                              \
00086   case ENOMEM:                                                          \
00087     ROS_ERROR("The process cannot allocate enough memory to create another mutex : %s:%d", __FILE__, __LINE__); \
00088     exit(1);                                                            \
00089     break;                                                              \
00090   case 0: /* SUCCESS */                                                 \
00091     break;                                                              \
00092   default:                                                              \
00093     ROS_ERROR("unknown error value, is this POSIX system ? : %s:%d", __FILE__, __LINE__); \
00094     exit(1);                                                            \
00095   }
00096 
00097 #define unlock(x)       switch ( pthread_mutex_unlock(x) )              \
00098   {                                                                     \
00099   case EINVAL:                                                          \
00100     ROS_ERROR("The value specified as a mutex is invalid : %s:%d", __FILE__, __LINE__); \
00101     exit(1);                                                            \
00102     break;                                                              \
00103   case EPERM:                                                           \
00104     ROS_ERROR("The current thread does not hold a lock on the mutex : %s:%d", __FILE__, __LINE__); \
00105     exit(1);                                                            \
00106     break;                                                              \
00107   }
00108 
00109 #define check_for_trylock_error(x)      if (x == EINVAL)        \
00110   {                                                             \
00111     ROS_ERROR("mutex error %s:%d", __FILE__, __LINE__);         \
00112     exit(1);                                                    \
00113   }
00114 
00115 
00122 SrEdc::SrEdc()
00123   : SR0X(),
00124     flashing(false),
00125     can_message_sent(true),
00126     can_packet_acked(true),
00127     can_bus_(0)
00128 {
00129   int res = 0;
00130   check_for_pthread_mutex_init_error(res);
00131   counter_ = 0;
00132 
00133   res = pthread_mutex_init(&producing, NULL);
00134   check_for_pthread_mutex_init_error(res);
00135 
00136   serviceServer = nodehandle_.advertiseService("SimpleMotorFlasher", &SrEdc::simple_motor_flasher, this);
00137 }
00138 
00143 SrEdc::~SrEdc()
00144 {
00145   delete sh_->get_fmmu_config();
00146   delete sh_->get_pd_config();
00147 }
00148 
00182 void SrEdc::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00183 {
00184     SR0X::construct(sh, start_address);
00185 }
00186 
00220 void SrEdc::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,
00221                                                  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)
00222 {
00223     SR0X::construct(sh, start_address);
00224 
00225     command_base_ = start_address;
00226     command_size_ = ethercat_command_data_size + ethercat_can_bridge_data_size;
00227 
00228     start_address += command_size_;
00229 
00230     status_base_ = start_address;
00231     status_size_ = ethercat_status_data_size + ethercat_can_bridge_data_size;
00232 
00233     start_address += status_size_;
00234 
00235     // ETHERCAT_COMMAND_DATA
00236     //
00237     // This is for data going TO the palm
00238     //
00239     ROS_INFO("First FMMU (command) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", command_base_, command_size_,
00240              static_cast<int>(ethercat_command_data_address) );
00241     EC_FMMU *commandFMMU = new EC_FMMU( command_base_,                                                  // Logical Start Address    (in ROS address space?)
00242                                         command_size_,
00243                                         0x00,                                                           // Logical Start Bit
00244                                         0x07,                                                           // Logical End Bit
00245                                         ethercat_command_data_address,                                  // Physical Start Address   (in ET1200 address space?)
00246                                         0x00,                                                           // Physical Start Bit
00247                                         false,                                                          // Read Enable
00248                                         true,                                                           // Write Enable
00249                                         true                                                            // Channel Enable
00250                                        );
00251 
00252 
00253 
00254 
00255     // ETHERCAT_STATUS_DATA
00256     //
00257     // This is for data coming FROM the palm
00258     //
00259     ROS_INFO("Second FMMU (status) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", status_base_, status_size_,
00260              static_cast<int>(ethercat_status_data_address) );
00261     EC_FMMU *statusFMMU = new EC_FMMU(  status_base_,
00262                                         status_size_,
00263                                         0x00,
00264                                         0x07,
00265                                         ethercat_status_data_address,
00266                                         0x00,
00267                                         true,
00268                                         false,
00269                                         true);
00270 
00271 
00272 
00273     EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00274 
00275     (*fmmu)[0] = *commandFMMU;
00276     (*fmmu)[1] = *statusFMMU;
00277 
00278     sh->set_fmmu_config(fmmu);
00279 
00280     EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(4);
00281 
00282     (*pd)[0] = EC_SyncMan(ethercat_command_data_address,             ethercat_command_data_size,    EC_QUEUED, EC_WRITTEN_FROM_MASTER);
00283     (*pd)[1] = EC_SyncMan(ethercat_can_bridge_data_command_address,  ethercat_can_bridge_data_size, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
00284     (*pd)[2] = EC_SyncMan(ethercat_status_data_address,              ethercat_status_data_size,     EC_QUEUED);
00285     (*pd)[3] = EC_SyncMan(ethercat_can_bridge_data_status_address,   ethercat_can_bridge_data_size, EC_QUEUED);
00286 
00287     status_size_ = ethercat_status_data_size + ethercat_can_bridge_data_size;
00288 
00289     (*pd)[0].ChannelEnable = true;
00290     (*pd)[0].ALEventEnable = true;
00291     (*pd)[0].WriteEvent    = true;
00292 
00293     (*pd)[1].ChannelEnable = true;
00294     (*pd)[1].ALEventEnable = true;
00295     (*pd)[1].WriteEvent    = true;
00296 
00297     (*pd)[2].ChannelEnable = true;
00298     (*pd)[3].ChannelEnable = true;
00299 
00300     sh->set_pd_config(pd);
00301 
00302     ROS_INFO("status_size_ : %d ; command_size_ : %d", status_size_, command_size_);
00303 }
00304 
00308 int SrEdc::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00309 {
00310 
00311   int retval = SR0X::initialize(hw, allow_unprogrammed);
00312 
00313   return retval;
00314 }
00315 
00321 void SrEdc::erase_flash(void)
00322 {
00323   unsigned char cmd_sent;
00324   unsigned int wait_time;
00325   bool timedout;
00326   unsigned int timeout;
00327   int err;
00328 
00329   do {
00330     ROS_INFO("Erasing FLASH");
00331     // First we send the erase command
00332     cmd_sent = 0;
00333     while (! cmd_sent )
00334     {
00335       if ( !(err = pthread_mutex_trylock(&producing)) )
00336       {
00337         can_message_.message_length = 1;
00338         can_message_.can_bus = can_bus_;
00339         can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | ERASE_FLASH_COMMAND;
00340         cmd_sent = 1;
00341         unlock(&producing);
00342       }
00343       else
00344       {
00345         check_for_trylock_error(err);
00346       }
00347     }
00348     wait_time = 0;
00349     timeout = 3000;
00350     can_message_sent = false;
00351     can_packet_acked = false;
00352     timedout = false;
00353     while ( !can_packet_acked )
00354     {
00355       usleep(1000);
00356       if (wait_time > timeout)
00357       {
00358         timedout = true;
00359         break;
00360       }
00361       wait_time++;
00362     }
00363 
00364     if (timedout)
00365     {
00366       ROS_ERROR("ERASE command timedout, resending it !");
00367     }
00368   } while (timedout);
00369 }
00370 
00387 bool SrEdc::read_flash(unsigned int offset, unsigned int baddr)
00388 {
00389   unsigned int cmd_sent;
00390   int err;
00391   unsigned int wait_time;
00392   bool timedout;
00393   unsigned int timeout;
00394   cmd_sent = 0;
00395   while ( !cmd_sent )
00396   {
00397     if ( !(err = pthread_mutex_trylock(&producing)) )
00398     {
00399       ROS_DEBUG("Sending READ data ... position : %03x", pos);
00400       can_message_.can_bus = can_bus_;
00401       can_message_.message_length = 3;
00402       can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | READ_FLASH_COMMAND;
00403       can_message_.message_data[2] = (offset + baddr) >> 16;
00404       can_message_.message_data[1] = (offset + baddr) >> 8; // User application start address is 0x4C0
00405       can_message_.message_data[0] = offset + baddr;
00406       cmd_sent = 1;
00407       unlock(&producing);
00408     }
00409     else
00410     {
00411       check_for_trylock_error(err);
00412     }
00413 
00414   }
00415   timedout = false;
00416   wait_time = 0;
00417   timeout = 100;
00418   can_message_sent = false;
00419   can_packet_acked = false;
00420   while ( !can_packet_acked )
00421   {
00422     usleep(1000);
00423     if (wait_time > timeout)
00424     {
00425       timedout = true;
00426       break;
00427     }
00428     wait_time++;
00429   }
00430   return timedout;
00431 }
00432 
00471 bool SrEdc::simple_motor_flasher(sr_robot_msgs::SimpleMotorFlasher::Request &req, sr_robot_msgs::SimpleMotorFlasher::Response &res)
00472 {
00473   bfd *fd;
00474   unsigned int base_addr;
00475   unsigned int smallest_start_address = 0x7fff;
00476   unsigned int biggest_end_address = 0;
00477   unsigned int total_size = 0;
00478   bool timedout;
00479 
00480   get_board_id_and_can_bus(req.motor_id, &can_bus_, &motor_being_flashed);
00481 
00482   binary_content = NULL;
00483   flashing = true;
00484 
00485   ROS_INFO("Flashing the motor");
00486 
00487   //Initialize the bfd library: "This routine must be called before any other BFD function to initialize magical internal data structures."
00488   bfd_init();
00489 
00490   //Open the requested firmware object file
00491   fd = bfd_openr(req.firmware.c_str(), NULL);
00492   if (fd == NULL)
00493   {
00494     ROS_ERROR("error opening the file %s", get_filename(req.firmware).c_str());
00495     res.value = res.FAIL;
00496     flashing = false;
00497     return false;
00498   }
00499 
00500   //Check that bfd recognises the file as a correctly formatted object file
00501   if (!bfd_check_format (fd, bfd_object))
00502   {
00503     if (bfd_get_error () != bfd_error_file_ambiguously_recognized)
00504     {
00505       ROS_ERROR("Incompatible format");
00506       res.value = res.FAIL;
00507       flashing = false;
00508       return false;
00509     }
00510   }
00511 
00512   ROS_INFO("firmware %s's format is : %s.", get_filename(req.firmware).c_str(), fd->xvec->name);
00513 
00514   //TODO Check if it's necessary to send this dummy packet before the magic packet
00515   ROS_DEBUG("Sending dummy packet");
00516   send_CAN_msg(can_bus_, 0, 0, NULL, 1, &timedout);
00517 
00518   ROS_INFO_STREAM("Switching motor "<< motor_being_flashed << " on CAN bus " << can_bus_ << " into bootloader mode");
00519   //Send the magic packet that will force the microcontroller to go into bootloader mode
00520   int8u magic_packet[] = {0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA};
00521   send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | 0b1010, 8, magic_packet, 100, &timedout);
00522 
00523   //Send a second magic packet if the first one wasn't acknowledged
00524   if ( timedout )
00525   {
00526     ROS_WARN("First magic CAN packet timedout");
00527     ROS_WARN("Sending another magic CAN packet to put the motor in bootloading mode");
00528     send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | 0b1010, 8, magic_packet, 100, &timedout);
00529 
00530     if (timedout)
00531     {
00532       ROS_ERROR("None of the magic packets were ACKed, didn't bootload the motor.");
00533       res.value = res.FAIL;
00534       flashing = false;
00535       return false;
00536     }
00537   }
00538 
00539   //Erase the PIC18 microcontroller flash memory
00540   erase_flash();
00541 
00542   sleep(1);
00543 
00544   //Look for the start and end address of every section in the hex file,
00545   //to detect the lowest and highest address of the data we need to write in the PIC's flash.
00546   find_address_range(fd, &smallest_start_address, &biggest_end_address);
00547 
00548   //Calculate the size of the chunk of data to be flashed
00549   total_size = biggest_end_address - smallest_start_address;
00550   base_addr = smallest_start_address;
00551 
00552   //Allocate the memory space to store the data to be flashed
00553   //This could be done with new bfd_byte[total_size+8] and delete() instead of malloc() and free() but will stay this way for the moment
00554   binary_content = (bfd_byte *)malloc(total_size+8);
00555   if (binary_content == NULL)
00556   {
00557     ROS_ERROR("Error allocating memory for binary_content");
00558     res.value = res.FAIL;
00559     flashing = false;
00560     return false;
00561   }
00562 
00563   //Set all the bytes in the binary_content to 0xFF initially (i.e. before reading the content from the hex file)
00564   //This way we make sure that any byte in the region between smallest_start_address and biggest_end_address
00565   //that is not included in any section of the hex file, will be written with a 0xFF value, which is the default in the PIC
00566   memset(binary_content, 0xFF, total_size+8);
00567 
00568   //The content of the firmware is read from the .hex file pointed by fd, to a memory region pointed by binary_content
00569   if(!read_content_from_object_file(fd, binary_content, base_addr))
00570   {
00571     ROS_ERROR("something went wrong while parsing %s.", get_filename(req.firmware).c_str());
00572     res.value = res.FAIL;
00573     free(binary_content);
00574     flashing = false;
00575     return false;
00576   }
00577 
00578   // We do not need the file anymore
00579   bfd_close(fd);
00580 
00581   //The firmware is actually written to the flash memory of the PIC18
00582   if(!write_flash_data(base_addr, total_size))
00583   {
00584     res.value = res.FAIL;
00585     free(binary_content);
00586     flashing = false;
00587     return false;
00588   }
00589 
00590 
00591   ROS_INFO("Verifying");
00592   // Now we have to read back the flash content
00593   if( !read_back_and_check_flash(base_addr, total_size))
00594   {
00595     res.value = res.FAIL;
00596     free(binary_content);
00597     flashing = false;
00598     return false;
00599   }
00600 
00601 
00602   free(binary_content);
00603 
00604   ROS_INFO("Resetting microcontroller.");
00605   // Then we send the RESET command to PIC18F
00606   do {
00607     send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | RESET_COMMAND, 0, NULL, 1000, &timedout);
00608   } while ( timedout );
00609 
00610 
00611   flashing = false;
00612 
00613   ROS_INFO("Flashing done");
00614 
00615   res.value = res.SUCCESS;
00616 
00617   //Reinitialize motor boards or valve controller boards information
00618   reinitialize_boards();
00619 
00620   return true;
00621 }
00622 
00623 void SrEdc::build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message)
00624 {
00625   int res;
00626 
00627   if (flashing && !can_packet_acked && !can_message_sent)
00628   {
00629     if ( !(res = pthread_mutex_trylock(&producing)) )
00630     {
00631       ROS_DEBUG_STREAM("Ethercat bridge data size: "<< ETHERCAT_CAN_BRIDGE_DATA_SIZE);
00632 
00633       ROS_DEBUG("We're sending a CAN message for flashing.");
00634       memcpy(message, &can_message_, sizeof(can_message_));
00635       can_message_sent = true;
00636 
00637       ROS_DEBUG("Sending : SID : 0x%04X ; bus : 0x%02X ; length : 0x%02X ; data : 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X",
00638                 message->message_id,
00639                 message->can_bus,
00640                 message->message_length,
00641                 message->message_data[0],
00642                 message->message_data[1],
00643                 message->message_data[2],
00644                 message->message_data[3],
00645                 message->message_data[4],
00646                 message->message_data[5],
00647                 message->message_data[6],
00648                 message->message_data[7]);
00649 
00650       unlock(&producing);
00651     }
00652     else
00653     {
00654       ROS_ERROR("Mutex is locked, we don't send any CAN message !");
00655       check_for_trylock_error(res);
00656     }
00657   }
00658   else
00659   {
00660     message->can_bus        = can_bus_;
00661     message->message_id     = 0x00;
00662     message->message_length = 0;
00663   }
00664 }
00665 
00674 bool SrEdc::can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA * packet)
00675 {
00676   int i;
00677 
00678   if (packet->message_id == 0)
00679   {
00680     ROS_DEBUG("ID is zero");
00681     return false;
00682   }
00683 
00684   ROS_DEBUG("ack sid : %04X", packet->message_id);
00685 
00686   // Is this a reply to a READ request?
00687   if ( (packet->message_id & 0b0000011111111111) == (0x0600 | (motor_being_flashed << 5) | 0x10 | READ_FLASH_COMMAND))
00688   {
00689     ROS_DEBUG("READ reply  %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", packet->message_data[0],
00690               packet->message_data[1],
00691               packet->message_data[2],
00692               packet->message_data[3],
00693               packet->message_data[4],
00694               packet->message_data[5],
00695               packet->message_data[6],
00696               packet->message_data[7]  );
00697     ROS_DEBUG("Should be   %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", binary_content[pos+0],
00698               binary_content[pos+1],
00699               binary_content[pos+2],
00700               binary_content[pos+3],
00701               binary_content[pos+4],
00702               binary_content[pos+5],
00703               binary_content[pos+6],
00704               binary_content[pos+7]  );
00705 
00706       if ( !memcmp(packet->message_data, binary_content + pos, 8) )
00707       {
00708         ROS_DEBUG("data is good");
00709         return true;
00710       }
00711       else
00712       {
00713         ROS_DEBUG("data is bad");
00714         return false;
00715       }
00716   }
00717 
00718   if (packet->message_length != can_message_.message_length)
00719   {
00720     ROS_DEBUG("Length is bad: %d", packet->message_length);
00721     return false;
00722   }
00723 
00724   ROS_DEBUG("Length is OK");
00725 
00726   for (i = 0 ; i < packet->message_length ; ++i)
00727   {
00728     ROS_DEBUG("packet sent, data[%d] : %02X ; ack, data[%d] : %02X", i, can_message_.message_data[i], i, packet->message_data[i]);
00729     if (packet->message_data[i] != can_message_.message_data[i])
00730       return false;
00731   }
00732   ROS_DEBUG("Data is OK");
00733 
00734   if ( !(0x0010 & packet->message_id))
00735     return false;
00736 
00737   ROS_DEBUG("This is an ACK");
00738 
00739   if ( (packet->message_id & 0b0000000111101111) != (can_message_.message_id & 0b0000000111101111) )
00740   {
00741     ROS_WARN_STREAM("Bad packet id: " << packet->message_id);
00742     return false;
00743   }
00744 
00745   ROS_DEBUG("SID is OK");
00746 
00747   ROS_DEBUG("Everything is OK, this is our ACK !");
00748   return true;
00749 }
00750 
00751 
00752 void SrEdc::send_CAN_msg(int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout)
00753 {
00754   int err;
00755   unsigned char cmd_sent;
00756   int wait_time;
00757 
00758   cmd_sent = 0;
00759   while ( !cmd_sent )
00760   {
00761     if ( !(err = pthread_mutex_trylock(&producing)) )
00762     {
00763       can_message_.message_length = msg_length;
00764       can_message_.can_bus = can_bus;
00765       can_message_.message_id = msg_id;
00766 
00767       if (msg_data != NULL)
00768       {
00769         for(unsigned int i = 0; i<msg_length; i++)
00770         {
00771           can_message_.message_data[i] = msg_data[i];
00772         }
00773       }
00774 
00775       cmd_sent = 1;
00776       unlock(&producing);
00777     }
00778     else
00779     {
00780       check_for_trylock_error(err);
00781     }
00782   }
00783   wait_time = 0;
00784   *timedout = false;
00785   can_message_sent = false;
00786   can_packet_acked = false;
00787   while ( !can_packet_acked )
00788   {
00789     usleep(1000); // 1 ms
00790     wait_time++;
00791     if (wait_time > timeout) {
00792       *timedout = true;
00793       break;
00794     }
00795   }
00796 }
00797 
00798 bool SrEdc::read_back_and_check_flash(unsigned int baddr, unsigned int total_size)
00799 {
00800   bool timedout;
00801   // The actual comparison between the content read from the flash and the content read from the hex file is carried out
00802   // in the can_data_is_ack() function.
00803   // read_flash(...) will return timedout = true if the 8 byte content read from the flash doesn't match the 8 bytes from the hex file
00804   //BE CAREFUL with the pos "global" field, because it's being used inside can_data_is_ack() function to check if the response
00805   //of the READ_FLASH_COMMAND is correct
00806   pos = 0;
00807   unsigned int retry;
00808   while (pos < total_size)
00809   {
00810     retry = 0;
00811     do {
00812       timedout = read_flash(pos, baddr);
00813       if ( ! timedout )
00814         pos += 8;
00815       retry++;
00816       if (retry > max_retry)
00817       {
00818         ROS_ERROR("Too much retry for READ back, try flashing again");
00819         return false;
00820       }
00821     } while ( timedout );
00822   }
00823   return true;
00824 }
00825 
00826 void SrEdc::find_address_range(bfd *fd, unsigned int *smallest_start_address, unsigned int *biggest_end_address)
00827 {
00828   asection *s;
00829   unsigned int section_size = 0;
00830   unsigned int section_addr = 0;
00831 
00832   //Look for the start and end address of every section in the hex file,
00833   //to detect the lowest and highest address of the data we need to write in the PIC's flash.
00834   //The sections starting at an address higher than 0x7fff will be ignored as they are not proper "code memory" firmware
00835   //(they can contain the CONFIG bits of the microcontroller, which we don't want to write here)
00836   //To understand the structure (sections) of the object file containing the firmware (usually a .hex) the following commands can be useful:
00837   //  \code objdump -x simplemotor.hex \endcode
00838   //  \code objdump -s simplemotor.hex \endcode
00839   for (s = fd->sections ; s ; s = s->next)
00840   {
00841     //Only the sections with the LOAD flag on will be considered
00842     if (bfd_get_section_flags (fd, s) & (SEC_LOAD))
00843     {
00844       //Only the sections with the same VMA (virtual memory address) and LMA (load MA) will be considered
00845       //http://www.delorie.com/gnu/docs/binutils/ld_7.html
00846       if (bfd_section_lma (fd, s) == bfd_section_vma (fd, s))
00847       {
00848         section_addr = (unsigned int) bfd_section_lma (fd, s);
00849         if (section_addr >= 0x7fff)
00850           continue;
00851         section_size = (unsigned int) bfd_section_size (fd, s);
00852         *smallest_start_address = min(section_addr, *smallest_start_address);
00853         *biggest_end_address = max(*biggest_end_address, section_addr + section_size);
00854       }
00855     }
00856   }
00857 }
00858 
00859 bool SrEdc::read_content_from_object_file(bfd *fd, bfd_byte *content, unsigned int base_addr)
00860 {
00861   asection *s;
00862   unsigned int section_size = 0;
00863   unsigned int section_addr = 0;
00864 
00865   for (s = fd->sections ; s ; s = s->next)
00866   {
00867     //Only the sections with the LOAD flag on will be considered
00868     if (bfd_get_section_flags (fd, s) & (SEC_LOAD))
00869     {
00870       //Only the sections with the same VMA (virtual memory address) and LMA (load MA) will be considered
00871       //http://www.delorie.com/gnu/docs/binutils/ld_7.html
00872       if (bfd_section_lma (fd, s) == bfd_section_vma (fd, s))
00873       {
00874         section_addr = (unsigned int) bfd_section_lma (fd, s);
00875         //The sections starting at an address higher than 0x7fff will be ignored as they are not proper "code memory" firmware
00876         //(they can contain the CONFIG bits of the microcontroller, which we don't want to write here)
00877         if (section_addr >= 0x7fff)
00878           continue;
00879         section_size = (unsigned int) bfd_section_size (fd, s);
00880         bfd_get_section_contents(fd, s, content + (section_addr - base_addr), 0, section_size);
00881       }
00882       else
00883       {
00884         return false;
00885       }
00886     }
00887     else
00888     {
00889       return false;
00890     }
00891   }
00892   return true;
00893 }
00894 
00895 bool SrEdc::write_flash_data(unsigned int base_addr, unsigned int total_size)
00896 {
00897   int err;
00898   unsigned char cmd_sent;
00899   int wait_time, timeout;
00900   bool timedout;
00901 
00902   pos = 0;
00903   unsigned int packet = 0;
00904   ROS_INFO("Sending the firmware data");
00905   while ( pos < ((total_size % 32) == 0 ? total_size : (total_size + 32 - (total_size % 32))) )
00906   {
00907     //For every WRITE_FLASH_ADDRESS_COMMAND we write 32 bytes of data to flash
00908     //and this is done by sending 4 WRITE_FLASH_DATA_COMMAND packets, every one containing 8 bytes of data to be written
00909     if ((pos % 32) == 0)
00910     {
00911       packet = 0;
00912       do {
00913         cmd_sent = 0;
00914         while (! cmd_sent )
00915         {
00916           if ( !(err = pthread_mutex_trylock(&producing)) )
00917           {
00918             can_message_.message_length = 3;
00919             can_message_.can_bus = can_bus_;
00920             can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | WRITE_FLASH_ADDRESS_COMMAND;
00921             can_message_.message_data[2] = (base_addr + pos) >> 16;
00922             can_message_.message_data[1] = (base_addr + pos) >> 8; // User application start address is 0x4C0
00923             can_message_.message_data[0] = base_addr + pos;
00924             ROS_DEBUG("Sending write address to motor %d : 0x%02X%02X%02X", motor_being_flashed, can_message_.message_data[2], can_message_.message_data[1], can_message_.message_data[0]);
00925             cmd_sent = 1;
00926             unlock(&producing);
00927           }
00928           else
00929           {
00930             check_for_trylock_error(err);
00931           }
00932         }
00933         wait_time = 0;
00934         timedout = false;
00935         timeout = 100;
00936         can_message_sent = false;
00937         can_packet_acked = false;
00938         while ( !can_packet_acked )
00939         {
00940           usleep(1000);
00941           if (wait_time > timeout)
00942           {
00943             timedout = true;
00944             break;
00945           }
00946           wait_time++;
00947         }
00948         if (timedout)
00949         {
00950           ROS_ERROR("WRITE ADDRESS timedout ");
00951         }
00952       } while ( timedout );
00953     }
00954     cmd_sent = 0;
00955     while (! cmd_sent )
00956     {
00957       if ( !(err = pthread_mutex_trylock(&producing)) )
00958       {
00959         ROS_DEBUG("Sending data ... position == %d", pos);
00960         can_message_.message_length = 8;
00961         can_message_.can_bus = can_bus_;
00962         can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | WRITE_FLASH_DATA_COMMAND;
00963         bzero(can_message_.message_data, 8);
00964         for (unsigned char j = 0 ; j < 8 ; ++j)
00965           can_message_.message_data[j] = (pos > total_size) ? 0xFF : *(binary_content + pos + j);
00966 
00967         pos += 8;
00968         cmd_sent = 1;
00969         unlock(&producing);
00970       }
00971       else
00972       {
00973         check_for_trylock_error(err);
00974       }
00975     }
00976     packet++;
00977     timedout = false;
00978     wait_time = 0;
00979     timeout = 100;
00980     can_message_sent = false;
00981     can_packet_acked = false;
00982     while ( !can_packet_acked )
00983     {
00984       usleep(1000);
00985       if (wait_time > timeout)
00986       {
00987         timedout = true;
00988         break;
00989       }
00990       wait_time++;
00991     }
00992     if ( timedout )
00993     {
00994       ROS_ERROR("A WRITE data packet has been lost at pos=%u, resending the 32 bytes block at pos=%u  !", pos, (pos - packet*8));
00995       pos -= packet*8;
00996     }
00997   }
00998   return true;
00999 }
01000 
01001 
01002 /* For the emacs weenies in the crowd.
01003    Local Variables:
01004    c-basic-offset: 2
01005    End:
01006 */


sr_edc_ethercat_drivers
Author(s): Yann Sionneau, Ugo Cupcic / ugo@shadowrobot.com, software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:38:25