sr06.cpp

Go to the documentation of this file.

#include <sr_edc_ethercat_drivers/sr06.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

#define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS)
#define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND)

const unsigned short int SR06::device_pub_freq_const      = 1000;
const unsigned short int SR06::ros_pub_freq_const         = 1000;
const unsigned short int SR06::max_iter_const             = device_pub_freq_const / ros_pub_freq_const;
const unsigned int       SR06::nb_sensors_const           = ETHERCAT_STATUS_DATA_SIZE/2; //36;
const unsigned char      SR06::nb_publish_by_unpack_const = (nb_sensors_const % max_iter_const) ? (nb_sensors_const / max_iter_const) + 1 : (nb_sensors_const / max_iter_const);
const unsigned int       SR06::max_retry                  = 20;

#define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)


PLUGINLIB_REGISTER_CLASS(6, SR06, EthercatDevice);

#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);                                                    \
  }


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

  ROS_INFO("There are %d sensors", nb_sensors_const);
  ROS_INFO(     "device_pub_freq_const = %d", device_pub_freq_const      );
  ROS_INFO(        "ros_pub_freq_const = %d", ros_pub_freq_const         );
  ROS_INFO(            "max_iter_const = %d", max_iter_const             );
  ROS_INFO(          "nb_sensors_const = %d", nb_sensors_const           );
  ROS_INFO("nb_publish_by_unpack_const = %d", nb_publish_by_unpack_const );

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

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

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

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

    command_base_ = start_address;
    command_size_ = ETHERCAT_COMMAND_DATA_SIZE + ETHERCAT_CAN_BRIDGE_DATA_SIZE;

    start_address += ETHERCAT_COMMAND_DATA_SIZE;
    start_address += ETHERCAT_CAN_BRIDGE_DATA_SIZE;

    status_base_ = start_address;
    status_size_ = ETHERCAT_STATUS_DATA_SIZE + ETHERCAT_CAN_BRIDGE_DATA_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_);

    ROS_INFO("Finished constructing the SR06 driver");
}

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

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

  if(retval != 0)
    return retval;

  sr_hand_lib = boost::shared_ptr<shadow_robot::SrHandLib>( new shadow_robot::SrHandLib(hw) );

  ROS_INFO("ETHERCAT_STATUS_DATA_SIZE      = %4d bytes", static_cast<int>(ETHERCAT_STATUS_DATA_SIZE) );
  ROS_INFO("ETHERCAT_COMMAND_DATA_SIZE     = %4d bytes", static_cast<int>(ETHERCAT_COMMAND_DATA_SIZE) );
  ROS_INFO("ETHERCAT_CAN_BRIDGE_DATA_SIZE  = %4d bytes", static_cast<int>(ETHERCAT_CAN_BRIDGE_DATA_SIZE) );

  //initialise the publisher for the extra analog inputs, gyroscope and accelerometer on the palm
  extra_analog_inputs_publisher.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray>(nodehandle_ , "palm_extras", 10));


  // Debug real time publisher: publishes the raw ethercat data
  debug_publisher = boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug>(nodehandle_ , "debug_etherCAT_data", 4));
  return retval;
}

void SR06::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 SR06::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 SR06::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;

  // We're using 2 can busses,
  // if motor id is between 0 and 9, then we're using the can_bus 1
  // else, we're using the can bus 2.
  int motor_id_tmp = req.motor_id;
  if( motor_id_tmp > 9 )
  {
    motor_id_tmp -= 10;
    can_bus_ = 2;
  }
  else
  {
    can_bus_ = 1;
  }

  motor_being_flashed = motor_id_tmp;
  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 motors information
  sr_hand_lib->reinitialize_motors();

  return true;
}

void SR06::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
{
  diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);

  stringstream name;
  d.name = "EtherCAT Dual CAN Palm";
  d.summary(d.OK, "OK");
  stringstream hwid;
  hwid << sh_->get_product_code() << "-" << sh_->get_serial();
  d.hardware_id = hwid.str();

  d.clear();
  d.addf("Position", "%02d", sh_->get_ring_position());
  d.addf("Product Code", "%d", sh_->get_product_code());
  d.addf("Serial Number", "%d", sh_->get_serial());
  d.addf("Revision", "%d", sh_->get_revision());
  d.addf("Counter", "%d", ++counter_);

  d.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
  d.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
  //reset the idle time min to a big number, to get a fresh number on next diagnostic
  sr_hand_lib->main_pic_idle_time_min = 1000;

  this->ethercatDiagnostics(d,2);
  vec.push_back(d);

  //Add the diagnostics from the hand
  sr_hand_lib->add_diagnostics(vec, d);

  //Add the diagnostics from the tactiles
  if( sr_hand_lib->tactiles != NULL )
    sr_hand_lib->tactiles->add_diagnostics(vec, d);
}



void SR06::packCommand(unsigned char *buffer, bool halt, bool reset)
{
  int res;

  SR0X::packCommand(buffer, halt, reset);

  ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND   *command = (ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND *)(buffer                             );
  ETHERCAT_CAN_BRIDGE_DATA                        *message = (ETHERCAT_CAN_BRIDGE_DATA                      *)(buffer + ETHERCAT_COMMAND_DATA_SIZE);

  if ( !flashing )
  {
    command->EDC_command = EDC_COMMAND_SENSOR_DATA;
  }
  else
  {
    command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
  }

  //alternate between even and uneven motors
  // and ask for the different informations.
  sr_hand_lib->build_motor_command(command);

  ROS_DEBUG("Sending command : Type : 0x%02X ; data : 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X",
                command->to_motor_data_type,
                command->motor_data[0],
                command->motor_data[1],
                command->motor_data[2],
                command->motor_data[3],
                command->motor_data[4],
                command->motor_data[5],
                command->motor_data[6],
                command->motor_data[7],
                command->motor_data[8],
                command->motor_data[9],
                command->motor_data[10],
                command->motor_data[11],
                command->motor_data[12],
                command->motor_data[13],
                command->motor_data[14],
                command->motor_data[15],
                command->motor_data[16],
                command->motor_data[17],
                command->motor_data[18],
                command->motor_data[19]);

  if (flashing && !can_packet_acked && !can_message_sent)
  {
    if ( !(res = pthread_mutex_trylock(&producing)) )
    {
      ROS_DEBUG_STREAM("Ethercat Command data size: "<< ETHERCAT_COMMAND_DATA_SIZE);
      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 SR06::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;
}

bool SR06::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
{
  ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS  *status_data = (ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS *)(this_buffer + command_size_                             );
  ETHERCAT_CAN_BRIDGE_DATA                      *can_data    = (ETHERCAT_CAN_BRIDGE_DATA                     *)(this_buffer + command_size_ + ETHERCAT_STATUS_DATA_SIZE );
  //  int16u                                        *status_buffer = (int16u*)status_data;
  static unsigned int num_rxed_packets = 0;

  ++num_rxed_packets;


  // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
  if(debug_publisher->trylock())
  {
    debug_publisher->msg_.header.stamp = ros::Time::now();

    debug_publisher->msg_.sensors.clear();
    for(unsigned int i=0; i<SENSORS_NUM_0220 + 1; ++i)
      debug_publisher->msg_.sensors.push_back( status_data->sensors[i] );

    debug_publisher->msg_.motor_data_type.data = static_cast<int>(status_data->motor_data_type);
    debug_publisher->msg_.which_motors = status_data->which_motors;
    debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
    debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;

    debug_publisher->msg_.motor_data_packet_torque.clear();
    debug_publisher->msg_.motor_data_packet_misc.clear();
    for(unsigned int i=0; i < 10; ++i)
    {
      debug_publisher->msg_.motor_data_packet_torque.push_back( status_data->motor_data_packet[i].torque );
      debug_publisher->msg_.motor_data_packet_misc.push_back( status_data->motor_data_packet[i].misc );
    }

    debug_publisher->msg_.tactile_data_type = static_cast<unsigned int>(static_cast<int32u>(status_data->tactile_data_type));
    debug_publisher->msg_.tactile_data_valid = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile_data_valid));
    debug_publisher->msg_.tactile.clear();
    for(unsigned int i=0; i < 5; ++i)
      debug_publisher->msg_.tactile.push_back( static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[i].word[0])) );

    debug_publisher->msg_.idle_time_us = status_data->idle_time_us;

    debug_publisher->unlockAndPublish();
  }

  if (status_data->EDC_command == EDC_COMMAND_INVALID)
  {
    //received empty message: the pic is not writing to its mailbox.
    ++zero_buffer_read;
    float percentage_packet_loss = 100.f * ((float)zero_buffer_read / (float)num_rxed_packets);

    ROS_DEBUG("Reception error detected : %d errors out of %d rxed packets (%2.3f%%) ; idle time %dus", zero_buffer_read, num_rxed_packets, percentage_packet_loss, status_data->idle_time_us);
    return true;
  }

  //We received a coherent message.
  //Update the library (positions, diagnostics values, actuators, etc...)
  //with the received information
  sr_hand_lib->update(status_data);

  //Now publish the additional data at 100Hz (every 10 cycles)
  if( cycle_count >= 9)
  {
    //publish tactiles if we have them
    if( sr_hand_lib->tactiles != NULL )
      sr_hand_lib->tactiles->publish();

    //And we also publish the additional data (accelerometer / gyroscope / analog inputs)
    std_msgs::Float64MultiArray extra_analog_msg;
    extra_analog_msg.layout.dim.resize(3);
    extra_analog_msg.data.resize(3+3+4);
    std::vector<double> data;

    extra_analog_msg.layout.dim[0].label = "accelerometer";
    extra_analog_msg.layout.dim[0].size = 3;
    extra_analog_msg.data[0] = status_data->sensors[ACCX];
    extra_analog_msg.data[1] = status_data->sensors[ACCY];
    extra_analog_msg.data[2] = status_data->sensors[ACCZ];

    extra_analog_msg.layout.dim[1].label = "gyrometer";
    extra_analog_msg.layout.dim[1].size = 3;
    extra_analog_msg.data[3] = status_data->sensors[GYRX];
    extra_analog_msg.data[4] = status_data->sensors[GYRY];
    extra_analog_msg.data[5] = status_data->sensors[GYRZ];

    extra_analog_msg.layout.dim[2].label = "analog_inputs";
    extra_analog_msg.layout.dim[2].size = 4;
    extra_analog_msg.data[6] = status_data->sensors[AN0];
    extra_analog_msg.data[7] = status_data->sensors[AN1];
    extra_analog_msg.data[8] = status_data->sensors[AN2];
    extra_analog_msg.data[9] = status_data->sensors[AN3];

    if( extra_analog_inputs_publisher->trylock() )
    {
      extra_analog_inputs_publisher->msg_ = extra_analog_msg;
      extra_analog_inputs_publisher->unlockAndPublish();
    }

    cycle_count = 0;
  }
  ++cycle_count;


  //If we're flashing, check is the packet has been acked
  if (flashing & !can_packet_acked)
  {
    if (can_data_is_ack(can_data))
    {
      can_packet_acked = true;
    }
  }

  return true;
}

void SR06::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 SR06::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 SR06::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 SR06::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 SR06::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 ");
          return false;
        }
      } 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, resending the 32 bytes block !");
      pos -= packet*8;
    }
  }
  return true;
}


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