sr_edc.cpp

Go to the documentation of this file.

#include <sr_edc_ethercat_drivers/sr_edc.h>

#include <dll/ethercat_dll.h>
#include <al/ethercat_AL.h>
#include <dll/ethercat_device_addressed_telegram.h>
#include <dll/ethercat_frame.h>
#include <realtime_tools/realtime_publisher.h>

#include <math.h>
#include <sstream>
#include <iomanip>
#include <boost/foreach.hpp>
#include <std_msgs/Int16.h>
#include <math.h>
#include <fcntl.h>
#include <stdio.h>
#include <pthread.h>
#include <bfd.h>

#include <sr_utilities/sr_math_utils.hpp>

using namespace std;

#include <sr_external_dependencies/types_for_external.h>
extern "C"
{
  #include <sr_external_dependencies/external/simplemotor-bootloader/bootloader.h>
}

#include <boost/static_assert.hpp>
namespace is_edc_command_32_bits
{
  //check is the EDC_COMMAND is 32bits on the computer
  //if not, fails
  BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
} // namespace is_edc_command_32_bits

const unsigned int       SrEdc::max_retry                  = 20;

#define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)



#define check_for_pthread_mutex_init_error(x)   switch(x)               \
  {                                                                     \
  case EAGAIN:                                                          \
    ROS_ERROR("The system temporarily lacks the resources to create another mutex : %s:%d", __FILE__, __LINE__); \
    exit(1);                                                            \
    break;                                                              \
  case EINVAL:                                                          \
    ROS_ERROR("The value specified as attribute is invalid for mutex init : %s:%d", __FILE__, __LINE__); \
    exit(1);                                                            \
    break;                                                              \
  case ENOMEM:                                                          \
    ROS_ERROR("The process cannot allocate enough memory to create another mutex : %s:%d", __FILE__, __LINE__); \
    exit(1);                                                            \
    break;                                                              \
  case 0: /* SUCCESS */                                                 \
    break;                                                              \
  default:                                                              \
    ROS_ERROR("unknown error value, is this POSIX system ? : %s:%d", __FILE__, __LINE__); \
    exit(1);                                                            \
  }

#define unlock(x)       switch ( pthread_mutex_unlock(x) )              \
  {                                                                     \
  case EINVAL:                                                          \
    ROS_ERROR("The value specified as a mutex is invalid : %s:%d", __FILE__, __LINE__); \
    exit(1);                                                            \
    break;                                                              \
  case EPERM:                                                           \
    ROS_ERROR("The current thread does not hold a lock on the mutex : %s:%d", __FILE__, __LINE__); \
    exit(1);                                                            \
    break;                                                              \
  }

#define check_for_trylock_error(x)      if (x == EINVAL)        \
  {                                                             \
    ROS_ERROR("mutex error %s:%d", __FILE__, __LINE__);         \
    exit(1);                                                    \
  }


SrEdc::SrEdc()
  : SR0X(),
    flashing(false),
    can_message_sent(true),
    can_packet_acked(true),
    can_bus_(0)
{
  int res = 0;
  check_for_pthread_mutex_init_error(res);
  counter_ = 0;

  res = pthread_mutex_init(&producing, NULL);
  check_for_pthread_mutex_init_error(res);

  serviceServer = nodehandle_.advertiseService("SimpleMotorFlasher", &SrEdc::simple_motor_flasher, this);
}

SrEdc::~SrEdc()
{
  delete sh_->get_fmmu_config();
  delete sh_->get_pd_config();
}

void SrEdc::construct(EtherCAT_SlaveHandler *sh, int &start_address)
{
    SR0X::construct(sh, start_address);
}

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,
                                                 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)
{
    SR0X::construct(sh, start_address);

    command_base_ = start_address;
    command_size_ = ethercat_command_data_size + ethercat_can_bridge_data_size;

    start_address += command_size_;

    status_base_ = start_address;
    status_size_ = ethercat_status_data_size + ethercat_can_bridge_data_size;

    start_address += status_size_;

    // ETHERCAT_COMMAND_DATA
    //
    // This is for data going TO the palm
    //
    ROS_INFO("First FMMU (command) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", command_base_, command_size_,
             static_cast<int>(ethercat_command_data_address) );
    EC_FMMU *commandFMMU = new EC_FMMU( command_base_,                                                  // Logical Start Address    (in ROS address space?)
                                        command_size_,
                                        0x00,                                                           // Logical Start Bit
                                        0x07,                                                           // Logical End Bit
                                        ethercat_command_data_address,                                  // Physical Start Address   (in ET1200 address space?)
                                        0x00,                                                           // Physical Start Bit
                                        false,                                                          // Read Enable
                                        true,                                                           // Write Enable
                                        true                                                            // Channel Enable
                                       );




    // ETHERCAT_STATUS_DATA
    //
    // This is for data coming FROM the palm
    //
    ROS_INFO("Second FMMU (status) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", status_base_, status_size_,
             static_cast<int>(ethercat_status_data_address) );
    EC_FMMU *statusFMMU = new EC_FMMU(  status_base_,
                                        status_size_,
                                        0x00,
                                        0x07,
                                        ethercat_status_data_address,
                                        0x00,
                                        true,
                                        false,
                                        true);



    EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);

    (*fmmu)[0] = *commandFMMU;
    (*fmmu)[1] = *statusFMMU;

    sh->set_fmmu_config(fmmu);

    EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(4);

    (*pd)[0] = EC_SyncMan(ethercat_command_data_address,             ethercat_command_data_size,    EC_QUEUED, EC_WRITTEN_FROM_MASTER);
    (*pd)[1] = EC_SyncMan(ethercat_can_bridge_data_command_address,  ethercat_can_bridge_data_size, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
    (*pd)[2] = EC_SyncMan(ethercat_status_data_address,              ethercat_status_data_size,     EC_QUEUED);
    (*pd)[3] = EC_SyncMan(ethercat_can_bridge_data_status_address,   ethercat_can_bridge_data_size, EC_QUEUED);

    status_size_ = ethercat_status_data_size + ethercat_can_bridge_data_size;

    (*pd)[0].ChannelEnable = true;
    (*pd)[0].ALEventEnable = true;
    (*pd)[0].WriteEvent    = true;

    (*pd)[1].ChannelEnable = true;
    (*pd)[1].ALEventEnable = true;
    (*pd)[1].WriteEvent    = true;

    (*pd)[2].ChannelEnable = true;
    (*pd)[3].ChannelEnable = true;

    sh->set_pd_config(pd);

    ROS_INFO("status_size_ : %d ; command_size_ : %d", status_size_, command_size_);
}

int SrEdc::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
{

  int retval = SR0X::initialize(hw, allow_unprogrammed);

  return retval;
}

void SrEdc::erase_flash(void)
{
  unsigned char cmd_sent;
  unsigned int wait_time;
  bool timedout;
  unsigned int timeout;
  int err;

  do {
    ROS_INFO("Erasing FLASH");
    // First we send the erase command
    cmd_sent = 0;
    while (! cmd_sent )
    {
      if ( !(err = pthread_mutex_trylock(&producing)) )
      {
        can_message_.message_length = 1;
        can_message_.can_bus = can_bus_;
        can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | ERASE_FLASH_COMMAND;
        cmd_sent = 1;
        unlock(&producing);
      }
      else
      {
        check_for_trylock_error(err);
      }
    }
    wait_time = 0;
    timeout = 3000;
    can_message_sent = false;
    can_packet_acked = false;
    timedout = false;
    while ( !can_packet_acked )
    {
      usleep(1000);
      if (wait_time > timeout)
      {
        timedout = true;
        break;
      }
      wait_time++;
    }

    if (timedout)
    {
      ROS_ERROR("ERASE command timedout, resending it !");
    }
  } while (timedout);
}

bool SrEdc::read_flash(unsigned int offset, unsigned int baddr)
{
  unsigned int cmd_sent;
  int err;
  unsigned int wait_time;
  bool timedout;
  unsigned int timeout;
  cmd_sent = 0;
  while ( !cmd_sent )
  {
    if ( !(err = pthread_mutex_trylock(&producing)) )
    {
      ROS_DEBUG("Sending READ data ... position : %03x", pos);
      can_message_.can_bus = can_bus_;
      can_message_.message_length = 3;
      can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | READ_FLASH_COMMAND;
      can_message_.message_data[2] = (offset + baddr) >> 16;
      can_message_.message_data[1] = (offset + baddr) >> 8; // User application start address is 0x4C0
      can_message_.message_data[0] = offset + baddr;
      cmd_sent = 1;
      unlock(&producing);
    }
    else
    {
      check_for_trylock_error(err);
    }

  }
  timedout = false;
  wait_time = 0;
  timeout = 100;
  can_message_sent = false;
  can_packet_acked = false;
  while ( !can_packet_acked )
  {
    usleep(1000);
    if (wait_time > timeout)
    {
      timedout = true;
      break;
    }
    wait_time++;
  }
  return timedout;
}

bool SrEdc::simple_motor_flasher(sr_robot_msgs::SimpleMotorFlasher::Request &req, sr_robot_msgs::SimpleMotorFlasher::Response &res)
{
  bfd *fd;
  unsigned int base_addr;
  unsigned int smallest_start_address = 0x7fff;
  unsigned int biggest_end_address = 0;
  unsigned int total_size = 0;
  bool timedout;

  get_board_id_and_can_bus(req.motor_id, &can_bus_, &motor_being_flashed);

  binary_content = NULL;
  flashing = true;

  ROS_INFO("Flashing the motor");

  //Initialize the bfd library: "This routine must be called before any other BFD function to initialize magical internal data structures."
  bfd_init();

  //Open the requested firmware object file
  fd = bfd_openr(req.firmware.c_str(), NULL);
  if (fd == NULL)
  {
    ROS_ERROR("error opening the file %s", get_filename(req.firmware).c_str());
    res.value = res.FAIL;
    flashing = false;
    return false;
  }

  //Check that bfd recognises the file as a correctly formatted object file
  if (!bfd_check_format (fd, bfd_object))
  {
    if (bfd_get_error () != bfd_error_file_ambiguously_recognized)
    {
      ROS_ERROR("Incompatible format");
      res.value = res.FAIL;
      flashing = false;
      return false;
    }
  }

  ROS_INFO("firmware %s's format is : %s.", get_filename(req.firmware).c_str(), fd->xvec->name);

  //TODO Check if it's necessary to send this dummy packet before the magic packet
  ROS_DEBUG("Sending dummy packet");
  send_CAN_msg(can_bus_, 0, 0, NULL, 1, &timedout);

  ROS_INFO_STREAM("Switching motor "<< motor_being_flashed << " on CAN bus " << can_bus_ << " into bootloader mode");
  //Send the magic packet that will force the microcontroller to go into bootloader mode
  int8u magic_packet[] = {0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA};
  send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | 0b1010, 8, magic_packet, 100, &timedout);

  //Send a second magic packet if the first one wasn't acknowledged
  if ( timedout )
  {
    ROS_WARN("First magic CAN packet timedout");
    ROS_WARN("Sending another magic CAN packet to put the motor in bootloading mode");
    send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | 0b1010, 8, magic_packet, 100, &timedout);

    if (timedout)
    {
      ROS_ERROR("None of the magic packets were ACKed, didn't bootload the motor.");
      res.value = res.FAIL;
      flashing = false;
      return false;
    }
  }

  //Erase the PIC18 microcontroller flash memory
  erase_flash();

  sleep(1);

  //Look for the start and end address of every section in the hex file,
  //to detect the lowest and highest address of the data we need to write in the PIC's flash.
  find_address_range(fd, &smallest_start_address, &biggest_end_address);

  //Calculate the size of the chunk of data to be flashed
  total_size = biggest_end_address - smallest_start_address;
  base_addr = smallest_start_address;

  //Allocate the memory space to store the data to be flashed
  //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
  binary_content = (bfd_byte *)malloc(total_size+8);
  if (binary_content == NULL)
  {
    ROS_ERROR("Error allocating memory for binary_content");
    res.value = res.FAIL;
    flashing = false;
    return false;
  }

  //Set all the bytes in the binary_content to 0xFF initially (i.e. before reading the content from the hex file)
  //This way we make sure that any byte in the region between smallest_start_address and biggest_end_address
  //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
  memset(binary_content, 0xFF, total_size+8);

  //The content of the firmware is read from the .hex file pointed by fd, to a memory region pointed by binary_content
  if(!read_content_from_object_file(fd, binary_content, base_addr))
  {
    ROS_ERROR("something went wrong while parsing %s.", get_filename(req.firmware).c_str());
    res.value = res.FAIL;
    free(binary_content);
    flashing = false;
    return false;
  }

  // We do not need the file anymore
  bfd_close(fd);

  //The firmware is actually written to the flash memory of the PIC18
  if(!write_flash_data(base_addr, total_size))
  {
    res.value = res.FAIL;
    free(binary_content);
    flashing = false;
    return false;
  }


  ROS_INFO("Verifying");
  // Now we have to read back the flash content
  if( !read_back_and_check_flash(base_addr, total_size))
  {
    res.value = res.FAIL;
    free(binary_content);
    flashing = false;
    return false;
  }


  free(binary_content);

  ROS_INFO("Resetting microcontroller.");
  // Then we send the RESET command to PIC18F
  do {
    send_CAN_msg(can_bus_, 0x0600 | (motor_being_flashed << 5) | RESET_COMMAND, 0, NULL, 1000, &timedout);
  } while ( timedout );


  flashing = false;

  ROS_INFO("Flashing done");

  res.value = res.SUCCESS;

  //Reinitialize motor boards or valve controller boards information
  reinitialize_boards();

  return true;
}

void SrEdc::build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message)
{
  int res;

  if (flashing && !can_packet_acked && !can_message_sent)
  {
    if ( !(res = pthread_mutex_trylock(&producing)) )
    {
      ROS_DEBUG_STREAM("Ethercat bridge data size: "<< ETHERCAT_CAN_BRIDGE_DATA_SIZE);

      ROS_DEBUG("We're sending a CAN message for flashing.");
      memcpy(message, &can_message_, sizeof(can_message_));
      can_message_sent = true;

      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",
                message->message_id,
                message->can_bus,
                message->message_length,
                message->message_data[0],
                message->message_data[1],
                message->message_data[2],
                message->message_data[3],
                message->message_data[4],
                message->message_data[5],
                message->message_data[6],
                message->message_data[7]);

      unlock(&producing);
    }
    else
    {
      ROS_ERROR("Mutex is locked, we don't send any CAN message !");
      check_for_trylock_error(res);
    }
  }
  else
  {
    message->can_bus        = can_bus_;
    message->message_id     = 0x00;
    message->message_length = 0;
  }
}

bool SrEdc::can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA * packet)
{
  int i;

  if (packet->message_id == 0)
  {
    ROS_DEBUG("ID is zero");
    return false;
  }

  ROS_DEBUG("ack sid : %04X", packet->message_id);

  // Is this a reply to a READ request?
  if ( (packet->message_id & 0b0000011111111111) == (0x0600 | (motor_being_flashed << 5) | 0x10 | READ_FLASH_COMMAND))
  {
    ROS_DEBUG("READ reply  %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", packet->message_data[0],
              packet->message_data[1],
              packet->message_data[2],
              packet->message_data[3],
              packet->message_data[4],
              packet->message_data[5],
              packet->message_data[6],
              packet->message_data[7]  );
    ROS_DEBUG("Should be   %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", binary_content[pos+0],
              binary_content[pos+1],
              binary_content[pos+2],
              binary_content[pos+3],
              binary_content[pos+4],
              binary_content[pos+5],
              binary_content[pos+6],
              binary_content[pos+7]  );

      if ( !memcmp(packet->message_data, binary_content + pos, 8) )
      {
        ROS_DEBUG("data is good");
        return true;
      }
      else
      {
        ROS_DEBUG("data is bad");
        return false;
      }
  }

  if (packet->message_length != can_message_.message_length)
  {
    ROS_DEBUG("Length is bad: %d", packet->message_length);
    return false;
  }

  ROS_DEBUG("Length is OK");

  for (i = 0 ; i < packet->message_length ; ++i)
  {
    ROS_DEBUG("packet sent, data[%d] : %02X ; ack, data[%d] : %02X", i, can_message_.message_data[i], i, packet->message_data[i]);
    if (packet->message_data[i] != can_message_.message_data[i])
      return false;
  }
  ROS_DEBUG("Data is OK");

  if ( !(0x0010 & packet->message_id))
    return false;

  ROS_DEBUG("This is an ACK");

  if ( (packet->message_id & 0b0000000111101111) != (can_message_.message_id & 0b0000000111101111) )
  {
    ROS_WARN_STREAM("Bad packet id: " << packet->message_id);
    return false;
  }

  ROS_DEBUG("SID is OK");

  ROS_DEBUG("Everything is OK, this is our ACK !");
  return true;
}


void SrEdc::send_CAN_msg(int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout)
{
  int err;
  unsigned char cmd_sent;
  int wait_time;

  cmd_sent = 0;
  while ( !cmd_sent )
  {
    if ( !(err = pthread_mutex_trylock(&producing)) )
    {
      can_message_.message_length = msg_length;
      can_message_.can_bus = can_bus;
      can_message_.message_id = msg_id;

      if (msg_data != NULL)
      {
        for(unsigned int i = 0; i<msg_length; i++)
        {
          can_message_.message_data[i] = msg_data[i];
        }
      }

      cmd_sent = 1;
      unlock(&producing);
    }
    else
    {
      check_for_trylock_error(err);
    }
  }
  wait_time = 0;
  *timedout = false;
  can_message_sent = false;
  can_packet_acked = false;
  while ( !can_packet_acked )
  {
    usleep(1000); // 1 ms
    wait_time++;
    if (wait_time > timeout) {
      *timedout = true;
      break;
    }
  }
}

bool SrEdc::read_back_and_check_flash(unsigned int baddr, unsigned int total_size)
{
  bool timedout;
  // The actual comparison between the content read from the flash and the content read from the hex file is carried out
  // in the can_data_is_ack() function.
  // 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
  //BE CAREFUL with the pos "global" field, because it's being used inside can_data_is_ack() function to check if the response
  //of the READ_FLASH_COMMAND is correct
  pos = 0;
  unsigned int retry;
  while (pos < total_size)
  {
    retry = 0;
    do {
      timedout = read_flash(pos, baddr);
      if ( ! timedout )
        pos += 8;
      retry++;
      if (retry > max_retry)
      {
        ROS_ERROR("Too much retry for READ back, try flashing again");
        return false;
      }
    } while ( timedout );
  }
  return true;
}

void SrEdc::find_address_range(bfd *fd, unsigned int *smallest_start_address, unsigned int *biggest_end_address)
{
  asection *s;
  unsigned int section_size = 0;
  unsigned int section_addr = 0;

  //Look for the start and end address of every section in the hex file,
  //to detect the lowest and highest address of the data we need to write in the PIC's flash.
  //The sections starting at an address higher than 0x7fff will be ignored as they are not proper "code memory" firmware
  //(they can contain the CONFIG bits of the microcontroller, which we don't want to write here)
  //To understand the structure (sections) of the object file containing the firmware (usually a .hex) the following commands can be useful:
  //  \code objdump -x simplemotor.hex \endcode
  //  \code objdump -s simplemotor.hex \endcode
  for (s = fd->sections ; s ; s = s->next)
  {
    //Only the sections with the LOAD flag on will be considered
    if (bfd_get_section_flags (fd, s) & (SEC_LOAD))
    {
      //Only the sections with the same VMA (virtual memory address) and LMA (load MA) will be considered
      //http://www.delorie.com/gnu/docs/binutils/ld_7.html
      if (bfd_section_lma (fd, s) == bfd_section_vma (fd, s))
      {
        section_addr = (unsigned int) bfd_section_lma (fd, s);
        if (section_addr >= 0x7fff)
          continue;
        section_size = (unsigned int) bfd_section_size (fd, s);
        *smallest_start_address = min(section_addr, *smallest_start_address);
        *biggest_end_address = max(*biggest_end_address, section_addr + section_size);
      }
    }
  }
}

bool SrEdc::read_content_from_object_file(bfd *fd, bfd_byte *content, unsigned int base_addr)
{
  asection *s;
  unsigned int section_size = 0;
  unsigned int section_addr = 0;

  for (s = fd->sections ; s ; s = s->next)
  {
    //Only the sections with the LOAD flag on will be considered
    if (bfd_get_section_flags (fd, s) & (SEC_LOAD))
    {
      //Only the sections with the same VMA (virtual memory address) and LMA (load MA) will be considered
      //http://www.delorie.com/gnu/docs/binutils/ld_7.html
      if (bfd_section_lma (fd, s) == bfd_section_vma (fd, s))
      {
        section_addr = (unsigned int) bfd_section_lma (fd, s);
        //The sections starting at an address higher than 0x7fff will be ignored as they are not proper "code memory" firmware
        //(they can contain the CONFIG bits of the microcontroller, which we don't want to write here)
        if (section_addr >= 0x7fff)
          continue;
        section_size = (unsigned int) bfd_section_size (fd, s);
        bfd_get_section_contents(fd, s, content + (section_addr - base_addr), 0, section_size);
      }
      else
      {
        return false;
      }
    }
    else
    {
      return false;
    }
  }
  return true;
}

bool SrEdc::write_flash_data(unsigned int base_addr, unsigned int total_size)
{
  int err;
  unsigned char cmd_sent;
  int wait_time, timeout;
  bool timedout;

  pos = 0;
  unsigned int packet = 0;
  ROS_INFO("Sending the firmware data");
  while ( pos < ((total_size % 32) == 0 ? total_size : (total_size + 32 - (total_size % 32))) )
  {
    //For every WRITE_FLASH_ADDRESS_COMMAND we write 32 bytes of data to flash
    //and this is done by sending 4 WRITE_FLASH_DATA_COMMAND packets, every one containing 8 bytes of data to be written
    if ((pos % 32) == 0)
    {
      packet = 0;
      do {
        cmd_sent = 0;
        while (! cmd_sent )
        {
          if ( !(err = pthread_mutex_trylock(&producing)) )
          {
            can_message_.message_length = 3;
            can_message_.can_bus = can_bus_;
            can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | WRITE_FLASH_ADDRESS_COMMAND;
            can_message_.message_data[2] = (base_addr + pos) >> 16;
            can_message_.message_data[1] = (base_addr + pos) >> 8; // User application start address is 0x4C0
            can_message_.message_data[0] = base_addr + pos;
            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]);
            cmd_sent = 1;
            unlock(&producing);
          }
          else
          {
            check_for_trylock_error(err);
          }
        }
        wait_time = 0;
        timedout = false;
        timeout = 100;
        can_message_sent = false;
        can_packet_acked = false;
        while ( !can_packet_acked )
        {
          usleep(1000);
          if (wait_time > timeout)
          {
            timedout = true;
            break;
          }
          wait_time++;
        }
        if (timedout)
        {
          ROS_ERROR("WRITE ADDRESS timedout ");
        }
      } while ( timedout );
    }
    cmd_sent = 0;
    while (! cmd_sent )
    {
      if ( !(err = pthread_mutex_trylock(&producing)) )
      {
        ROS_DEBUG("Sending data ... position == %d", pos);
        can_message_.message_length = 8;
        can_message_.can_bus = can_bus_;
        can_message_.message_id = 0x0600 | (motor_being_flashed << 5) | WRITE_FLASH_DATA_COMMAND;
        bzero(can_message_.message_data, 8);
        for (unsigned char j = 0 ; j < 8 ; ++j)
          can_message_.message_data[j] = (pos > total_size) ? 0xFF : *(binary_content + pos + j);

        pos += 8;
        cmd_sent = 1;
        unlock(&producing);
      }
      else
      {
        check_for_trylock_error(err);
      }
    }
    packet++;
    timedout = false;
    wait_time = 0;
    timeout = 100;
    can_message_sent = false;
    can_packet_acked = false;
    while ( !can_packet_acked )
    {
      usleep(1000);
      if (wait_time > timeout)
      {
        timedout = true;
        break;
      }
      wait_time++;
    }
    if ( timedout )
    {
      ROS_ERROR("A WRITE data packet has been lost at pos=%u, resending the 32 bytes block at pos=%u  !", pos, (pos - packet*8));
      pos -= packet*8;
    }
  }
  return true;
}


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