wg0x.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <iomanip>
00036 
00037 #include <math.h>
00038 #include <stddef.h>
00039 
00040 #include <ethercat_hardware/wg0x.h>
00041 
00042 #include <dll/ethercat_dll.h>
00043 #include <al/ethercat_AL.h>
00044 #include <dll/ethercat_device_addressed_telegram.h>
00045 #include <dll/ethercat_frame.h>
00046 
00047 #include <boost/crc.hpp>
00048 #include <boost/static_assert.hpp>
00049 #include <boost/make_shared.hpp>
00050 
00051 // Temporary,, need 'log' fuction that can switch between fprintf and ROS_LOG.
00052 #define ERR_MODE "\033[41m"
00053 #define STD_MODE "\033[0m"
00054 #define WARN_MODE "\033[43m"
00055 #define GOOD_MODE "\033[42m"
00056 #define INFO_MODE "\033[44m"
00057 
00058 #define ERROR_HDR "\033[41mERROR\033[0m"
00059 #define WARN_HDR "\033[43mERROR\033[0m"
00060 
00061 #include "ethercat_hardware/wg_util.h"
00062 
00063 
00064 WG0XDiagnostics::WG0XDiagnostics() :
00065   first_(true),
00066   valid_(false),
00067   safety_disable_total_(0),
00068   undervoltage_total_(0),
00069   over_current_total_(0),
00070   board_over_temp_total_(0),
00071   bridge_over_temp_total_(0),
00072   operate_disable_total_(0),
00073   watchdog_disable_total_(0),
00074   lock_errors_(0),
00075   checksum_errors_(0),
00076   zero_offset_(0),
00077   cached_zero_offset_(0)
00078 {
00079   memset(&safety_disable_status_, 0, sizeof(safety_disable_status_));
00080   memset(&diagnostics_info_, 0, sizeof(diagnostics_info_));
00081 }
00082 
00089 void WG0XDiagnostics::update(const WG0XSafetyDisableStatus &new_status, const WG0XDiagnosticsInfo &new_diagnostics_info)
00090 {
00091   first_ = false;
00092   safety_disable_total_   += 0xFF & ((uint32_t)(new_status.safety_disable_count_ - safety_disable_status_.safety_disable_count_));
00093   {
00094     const WG0XSafetyDisableCounters &new_counters(new_diagnostics_info.safety_disable_counters_);
00095     const WG0XSafetyDisableCounters &old_counters(diagnostics_info_.safety_disable_counters_);
00096     undervoltage_total_     += 0xFF & ((uint32_t)(new_counters.undervoltage_count_ - old_counters.undervoltage_count_));
00097     over_current_total_     += 0xFF & ((uint32_t)(new_counters.over_current_count_ - old_counters.over_current_count_));
00098     board_over_temp_total_  += 0xFF & ((uint32_t)(new_counters.board_over_temp_count_ - old_counters.board_over_temp_count_));
00099     bridge_over_temp_total_ += 0xFF & ((uint32_t)(new_counters.bridge_over_temp_count_ - old_counters.bridge_over_temp_count_));
00100     operate_disable_total_  += 0xFF & ((uint32_t)(new_counters.operate_disable_count_ - old_counters.operate_disable_count_));
00101     watchdog_disable_total_ += 0xFF & ((uint32_t)(new_counters.watchdog_disable_count_ - old_counters.watchdog_disable_count_));
00102   } 
00103 
00104   safety_disable_status_   = new_status;
00105   diagnostics_info_        = new_diagnostics_info;
00106 }
00107 
00108 
00130 bool WG0XActuatorInfo::verifyCRC() const
00131 {
00132   // Actuator info contains two CRCs
00133   BOOST_STATIC_ASSERT(sizeof(WG0XActuatorInfo) == 264);
00134   BOOST_STATIC_ASSERT( offsetof(WG0XActuatorInfo, crc32_256_) == (256-4));
00135   BOOST_STATIC_ASSERT( offsetof(WG0XActuatorInfo, crc32_264_) == (264-4));
00136   boost::crc_32_type crc32_256, crc32_264;  
00137   crc32_256.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_256_));
00138   crc32_264.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_264_));
00139   return ((this->crc32_264_ == crc32_264.checksum()) || (this->crc32_256_ == crc32_256.checksum()));
00140 }
00141 
00145 void WG0XActuatorInfo::generateCRC(void)
00146 {
00147   boost::crc_32_type crc32;
00148   crc32.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_256_));
00149   this->crc32_256_ = crc32.checksum();
00150   crc32.reset();
00151   crc32.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_264_));
00152   this->crc32_264_ = crc32.checksum();
00153 }
00154 
00155 
00156 WG0X::WG0X() :
00157   max_current_(0.0),
00158   too_many_dropped_packets_(false),
00159   status_checksum_error_(false),
00160   timestamp_jump_detected_(false),
00161   fpga_internal_reset_detected_(false),
00162   encoder_errors_detected_(false),
00163   cached_zero_offset_(0), 
00164   calibration_status_(NO_CALIBRATION),
00165   app_ram_status_(APP_RAM_MISSING),
00166   motor_model_(NULL),
00167   disable_motor_model_checking_(false)
00168 {
00169 
00170   last_timestamp_ = 0;
00171   last_last_timestamp_ = 0;
00172   drops_ = 0;
00173   consecutive_drops_ = 0;
00174   max_consecutive_drops_ = 0;
00175   max_board_temperature_ = 0;
00176   max_bridge_temperature_ = 0;
00177   in_lockout_ = false;
00178   resetting_ = false;
00179   has_error_ = false;
00180 
00181   int error;
00182   if ((error = pthread_mutex_init(&wg0x_diagnostics_lock_, NULL)) != 0)
00183   {
00184     ROS_ERROR("WG0X : init diagnostics mutex :%s", strerror(error));
00185   }
00186 
00187 }
00188 
00189 WG0X::~WG0X()
00190 {
00191   delete sh_->get_fmmu_config();
00192   delete sh_->get_pd_config();
00193   delete motor_model_;
00194 }
00195 
00196 
00197 void WG0X::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00198 {
00199   EthercatDevice::construct(sh, start_address);
00200 
00201   // WG EtherCAT devices (WG05,WG06,WG21) revisioning scheme
00202   fw_major_ = (sh->get_revision() >> 8) & 0xff;
00203   fw_minor_ = sh->get_revision() & 0xff;
00204   board_major_ = ((sh->get_revision() >> 24) & 0xff) - 1;
00205   board_minor_ = (sh->get_revision() >> 16) & 0xff;
00206 
00207   // Would normally configure EtherCAT initialize EtherCAT communication settings here.
00208   // However, since all WG devices are slightly different doesn't make sense to do it here.
00209   // Instead make sub-classes handle this.
00210 }
00211 
00212 
00219 void WG0X::copyActuatorInfo(ethercat_hardware::ActuatorInfo &out,  const WG0XActuatorInfo &in)
00220 {
00221   out.id   = in.id_;
00222   out.name = std::string(in.name_);
00223   out.robot_name = in.robot_name_;
00224   out.motor_make = in.motor_make_;
00225   out.motor_model = in.motor_model_;
00226   out.max_current = in.max_current_;
00227   out.speed_constant = in.speed_constant_; 
00228   out.motor_resistance  = in.resistance_;
00229   out.motor_torque_constant = in.motor_torque_constant_;
00230   out.encoder_reduction = in.encoder_reduction_;
00231   out.pulses_per_revolution = in.pulses_per_revolution_;  
00232 }
00233 
00234 
00237 bool WG0X::initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw, 
00238                                 const string &device_description,
00239                                 double max_pwm_ratio, 
00240                                 double board_resistance,
00241                                 bool   poor_measured_motor_voltage)
00242 {
00243   if (!hw) 
00244     return true;
00245 
00246   motor_model_ = new MotorModel(1000);
00247   if (motor_model_ == NULL) 
00248     return false;
00249 
00250   const ethercat_hardware::ActuatorInfo &ai(actuator_info_msg_);
00251   
00252   unsigned product_code = sh_->get_product_code();
00253   ethercat_hardware::BoardInfo bi;
00254   bi.description = device_description; 
00255   bi.product_code = product_code;
00256   bi.pcb = board_major_;
00257   bi.pca = board_minor_;
00258   bi.serial = sh_->get_serial();
00259   bi.firmware_major = fw_major_;
00260   bi.firmware_minor = fw_minor_;
00261   bi.board_resistance = board_resistance;
00262   bi.max_pwm_ratio    = max_pwm_ratio;
00263   bi.hw_max_current   = config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_;
00264   bi.poor_measured_motor_voltage = poor_measured_motor_voltage;
00265 
00266   if (!motor_model_->initialize(ai,bi))
00267     return false;
00268   
00269   // Create digital out that can be used to force trigger of motor trace
00270   publish_motor_trace_.name_ = string(actuator_info_.name_) + "_publish_motor_trace";
00271   publish_motor_trace_.command_.data_ = 0;
00272   publish_motor_trace_.state_.data_ = 0;
00273   if (!hw->addDigitalOut(&publish_motor_trace_)) {
00274     ROS_FATAL("A digital out of the name '%s' already exists", publish_motor_trace_.name_.c_str());
00275     return false;
00276   }
00277 
00278   // When working with experimental setups we don't want motor model to halt motors when it detects a problem.
00279   // Allow rosparam to disable motor model halting for a specific motor.
00280   if (!ros::param::get("~/" + ai.name + "/disable_motor_model_checking", disable_motor_model_checking_))
00281   {
00282     disable_motor_model_checking_ = false;
00283   }
00284   if (disable_motor_model_checking_)
00285   {
00286     ROS_WARN("Disabling motor model on %s", ai.name.c_str());
00287   }
00288 
00289   return true;
00290 }
00291 
00292 
00293 boost::shared_ptr<ethercat_hardware::MotorHeatingModelCommon> WG0X::motor_heating_model_common_;
00294 
00295 bool WG0X::initializeMotorHeatingModel(bool allow_unprogrammed)
00296 {
00297 
00298   EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00299   ethercat_hardware::MotorHeatingModelParametersEepromConfig config;
00300   if (!readMotorHeatingModelParametersFromEeprom(&com, config))
00301   {
00302     ROS_FATAL("Unable to read motor heating model config parameters from EEPROM");
00303     return false;
00304   }
00305 
00306   // All devices need to have motor model heating model parameters stored in them...
00307   // Even if device doesn't use paramers, they should be there.
00308   if (!config.verifyCRC())
00309   {
00310     if (allow_unprogrammed)
00311     {
00312       ROS_WARN("%s EEPROM does not contain motor heating model parameters",
00313                actuator_info_.name_);
00314       return true;
00315     }
00316     else 
00317     {
00318       ROS_WARN("%s EEPROM does not contain motor heating model parameters",
00319                actuator_info_.name_);
00320       return true;
00321       // TODO: once there is ability to update all MCB iwth motorconf, this is will become a fatal error
00322       ROS_FATAL("%s EEPROM does not contain motor heating model parameters", 
00323                 actuator_info_.name_);
00324       return false;
00325     }
00326   }
00327 
00328   // Even though all devices should contain motor heating model parameters,
00329   // The heating model does not need to be used.
00330   if (config.enforce_ == 0)
00331   {
00332     return true;
00333   }
00334 
00335   // Don't need motor model if we are not using ROS (motorconf)
00336   if (!use_ros_)
00337   {
00338     return true;
00339   }
00340 
00341   // Generate hwid for motor model
00342   std::ostringstream hwid;
00343   hwid << unsigned(sh_->get_product_code()) << std::setw(5) << std::setfill('0') << unsigned(sh_->get_serial());
00344 
00345   // All motor heating models use shared settings structure
00346   if (motor_heating_model_common_.get() == NULL)
00347   {
00348     ros::NodeHandle nh("~motor_heating_model");
00349     motor_heating_model_common_ = boost::make_shared<ethercat_hardware::MotorHeatingModelCommon>(nh);
00350     motor_heating_model_common_->initialize();
00351     // Only display following warnings once.
00352     if (!motor_heating_model_common_->enable_model_)
00353     {
00354       ROS_WARN("Motor heating model disabled for all devices");
00355     }
00356     if (!motor_heating_model_common_->load_save_files_)
00357     {
00358       ROS_WARN("Not loading motor heating model files");
00359     }
00360     if (!motor_heating_model_common_->update_save_files_)
00361     {
00362       ROS_WARN("Not saving motor heating model files");
00363     }
00364   }
00365 
00366   if (!motor_heating_model_common_->enable_model_)
00367   {
00368     return true;
00369   }
00370     
00371   motor_heating_model_ = 
00372     boost::make_shared<ethercat_hardware::MotorHeatingModel>(config.params_, 
00373                                                              actuator_info_.name_, 
00374                                                              hwid.str(),
00375                                                              motor_heating_model_common_->save_directory_); 
00376   // have motor heating model load last saved temperaures from filesystem
00377   if (motor_heating_model_common_->load_save_files_)
00378   {
00379     if (!motor_heating_model_->loadTemperatureState())
00380     {
00381       ROS_WARN("Could not load motor temperature state for %s", actuator_info_.name_);
00382     }
00383   }
00384   if (motor_heating_model_common_->publish_temperature_)
00385   {
00386     motor_heating_model_->startTemperaturePublisher();
00387   }
00388   motor_heating_model_common_->attach(motor_heating_model_);
00389 
00390   return true;
00391 }
00392 
00393 
00394 int WG0X::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00395 {
00396   ROS_DEBUG("Device #%02d: WG0%d (%#08x) Firmware Revision %d.%02d, PCB Revision %c.%02d, Serial #: %d", 
00397             sh_->get_ring_position(),
00398             sh_->get_product_code() % 100,
00399             sh_->get_product_code(), fw_major_, fw_minor_,
00400             'A' + board_major_, board_minor_,
00401             sh_->get_serial());
00402 
00403   EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00404 
00405   mailbox_.initialize(sh_);
00406 
00407   if (sh_->get_product_code() == WG05_PRODUCT_CODE)
00408   {
00409     if (fw_major_ != 1 || fw_minor_ < 7)
00410     {
00411       ROS_FATAL("Unsupported firmware revision %d.%02d", fw_major_, fw_minor_);
00412       return -1;
00413     }
00414   }
00415   else
00416   {
00417     if ((fw_major_ == 0 && fw_minor_ < 4) /*|| (fw_major_ == 1 && fw_minor_ < 0)*/)
00418     {
00419       ROS_FATAL("Unsupported firmware revision %d.%02d", fw_major_, fw_minor_);
00420       return -1;
00421     }
00422   }
00423 
00424   if (readMailbox(&com, WG0XConfigInfo::CONFIG_INFO_BASE_ADDR, &config_info_, sizeof(config_info_)) != 0)
00425   {
00426     ROS_FATAL("Unable to load configuration information");
00427     return -1;
00428   }
00429   ROS_DEBUG("            Serial #: %05d", config_info_.device_serial_number_);
00430   double board_max_current = double(config_info_.absolute_current_limit_) * config_info_.nominal_current_scale_;
00431 
00432   if (!readActuatorInfoFromEeprom(&com, actuator_info_))
00433   {
00434     ROS_FATAL("Unable to read actuator info from EEPROM");
00435     return -1;
00436   }
00437   
00438   if (actuator_info_.verifyCRC())
00439   {
00440     if (actuator_info_.major_ != 0 || actuator_info_.minor_ != 2)
00441     {
00442       if (allow_unprogrammed)
00443         ROS_WARN("Unsupported actuator info version (%d.%d != 0.2).  Please reprogram device #%02d", actuator_info_.major_, actuator_info_.minor_, sh_->get_ring_position());
00444       else
00445       {
00446         ROS_FATAL("Unsupported actuator info version (%d.%d != 0.2).  Please reprogram device #%02d", actuator_info_.major_, actuator_info_.minor_, sh_->get_ring_position());
00447         return -1;
00448       }
00449     }
00450 
00451     actuator_.name_ = actuator_info_.name_;
00452     ROS_DEBUG("            Name: %s", actuator_info_.name_);
00453 
00454     // Copy actuator info read from eeprom, into msg type
00455     copyActuatorInfo(actuator_info_msg_, actuator_info_);
00456 
00457     if (!initializeMotorHeatingModel(allow_unprogrammed))
00458     {
00459       return -1;
00460     }
00461 
00462 
00463     bool isWG021 = sh_->get_product_code() == WG021_PRODUCT_CODE;
00464     if (!isWG021)
00465     {
00466       // Register actuator with pr2_hardware_interface::HardwareInterface
00467       if (hw && !hw->addActuator(&actuator_))
00468       {
00469           ROS_FATAL("An actuator of the name '%s' already exists.  Device #%02d has a duplicate name", actuator_.name_.c_str(), sh_->get_ring_position());
00470           return -1;
00471       }
00472 
00473     }
00474 
00475     // Register digital out with pr2_hardware_interface::HardwareInterface
00476     digital_out_.name_ = actuator_info_.name_;
00477     if (hw && !hw->addDigitalOut(&digital_out_))
00478     {
00479         ROS_FATAL("A digital out of the name '%s' already exists.  Device #%02d has a duplicate name", digital_out_.name_.c_str(), sh_->get_ring_position());
00480         return -1;
00481     }
00482 
00483     // If it is supported, read application ram data.
00484     if (app_ram_status_ == APP_RAM_PRESENT)
00485     {
00486       double zero_offset;
00487       if (readAppRam(&com, zero_offset))
00488       {
00489         ROS_DEBUG("Read calibration from device %s: %f", actuator_info_.name_, zero_offset);
00490         actuator_.state_.zero_offset_ = zero_offset;
00491         cached_zero_offset_ = zero_offset;
00492         calibration_status_ = SAVED_CALIBRATION;
00493       }
00494       else
00495       {
00496         ROS_DEBUG("No calibration offset was stored on device %s", actuator_info_.name_);
00497       }
00498     }
00499     else if (app_ram_status_ == APP_RAM_MISSING)
00500     {
00501       ROS_WARN("Device %s does not support storing calibration offsets", actuator_info_.name_);
00502     }
00503     else if (app_ram_status_ == APP_RAM_NOT_APPLICABLE)
00504     {
00505       // don't produce warning
00506     }
00507 
00508     // Make sure motor current limit is less than board current limit
00509     if (actuator_info_.max_current_ > board_max_current)
00510     {
00511       ROS_WARN("WARNING: Device #%02d : motor current limit (%f) greater than board current limit (%f)", 
00512                sh_->get_ring_position(), actuator_info_.max_current_, board_max_current);
00513     }
00514     max_current_ = std::min(board_max_current, actuator_info_.max_current_);
00515   }
00516   else if (allow_unprogrammed)
00517   {
00518     ROS_WARN("WARNING: Device #%02d (%d%05d) is not programmed", 
00519              sh_->get_ring_position(), sh_->get_product_code(), sh_->get_serial());
00520     //actuator_info_.crc32_264_ = 0;
00521     //actuator_info_.crc32_256_ = 0;
00522 
00523     max_current_ = board_max_current;
00524   }
00525   else
00526   {
00527     ROS_FATAL("Device #%02d (%d%05d) is not programmed, aborting...", 
00528               sh_->get_ring_position(), sh_->get_product_code(), sh_->get_serial());
00529     return -1;
00530   }
00531 
00532   return 0;
00533 }
00534 
00535 #define GET_ATTR(a) \
00536 { \
00537   TiXmlElement *c; \
00538   attr = elt->Attribute((a)); \
00539   if (!attr) { \
00540     c = elt->FirstChildElement((a)); \
00541     if (!c || !(attr = c->GetText())) { \
00542       ROS_FATAL("Actuator is missing the attribute "#a); \
00543       exit(EXIT_FAILURE); \
00544     } \
00545   } \
00546 }
00547 
00548 void WG0X::clearErrorFlags(void)
00549 {
00550   has_error_ = false;
00551   too_many_dropped_packets_ = false;
00552   status_checksum_error_ = false;
00553   timestamp_jump_detected_ = false;
00554   encoder_errors_detected_ = false;
00555   if (motor_model_) motor_model_->reset();
00556   if (motor_heating_model_.get() != NULL) 
00557   {
00558     motor_heating_model_->reset();
00559   }
00560 }
00561 
00562 void WG0X::packCommand(unsigned char *buffer, bool halt, bool reset)
00563 {
00564   pr2_hardware_interface::ActuatorCommand &cmd = actuator_.command_;
00565   
00566   if (halt) 
00567   {
00568     cmd.effort_ = 0;
00569   }
00570 
00571   if (reset) 
00572   {
00573     clearErrorFlags();
00574   }
00575   resetting_ = reset;
00576 
00577   // If zero_offset was changed, give it to non-realtime thread
00578   double zero_offset = actuator_.state_.zero_offset_;
00579   if (zero_offset != cached_zero_offset_) 
00580   {
00581     if (tryLockWG0XDiagnostics())
00582     {
00583       ROS_DEBUG("Calibration change of %s, new %f, old %f", actuator_info_.name_, zero_offset, cached_zero_offset_);
00584       cached_zero_offset_ = zero_offset;
00585       wg0x_collect_diagnostics_.zero_offset_ = zero_offset;
00586       calibration_status_ = CONTROLLER_CALIBRATION;
00587       unlockWG0XDiagnostics();
00588     }
00589     else 
00590     {
00591       // It is OK if trylock failed, this will still try again next cycle.
00592     }
00593   }
00594 
00595   // Compute the current
00596   double current = (cmd.effort_ / actuator_info_.encoder_reduction_) / actuator_info_.motor_torque_constant_ ;
00597   actuator_.state_.last_commanded_effort_ = cmd.effort_;
00598   actuator_.state_.last_commanded_current_ = current;
00599 
00600   // Truncate the current to limit
00601   current = max(min(current, max_current_), -max_current_);
00602 
00603   // Pack command structures into EtherCAT buffer
00604   WG0XCommand *c = (WG0XCommand *)buffer;
00605   memset(c, 0, command_size_);
00606   c->programmed_current_ = int(current / config_info_.nominal_current_scale_);
00607   c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
00608   c->mode_ |= (reset ? MODE_SAFETY_RESET : 0);
00609   c->digital_out_ = digital_out_.command_.data_;
00610   c->checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(c, command_size_ - 1));
00611 }
00612 
00613 bool WG0X::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00614 {
00615   pr2_hardware_interface::ActuatorState &state = actuator_.state_;
00616   WG0XStatus *this_status, *prev_status;
00617 
00618   this_status = (WG0XStatus *)(this_buffer + command_size_);
00619   prev_status = (WG0XStatus *)(prev_buffer + command_size_);
00620 
00621   digital_out_.state_.data_ = this_status->digital_out_;
00622 
00623   // Do not report timestamp directly to controllers because 32bit integer 
00624   // value in microseconds will overflow every 72 minutes.   
00625   // Instead a accumulate small time differences into a ros::Duration variable
00626   int32_t timediff = WG0X::timestampDiff(this_status->timestamp_, prev_status->timestamp_);
00627   sample_timestamp_ += WG0X::timediffToDuration(timediff);
00628   state.sample_timestamp_ = sample_timestamp_;   //ros::Duration is preferred source of time for controllers
00629   state.timestamp_ = sample_timestamp_.toSec();  //double value is for backwards compatibility
00630   
00631   state.device_id_ = sh_->get_ring_position();
00632   
00633   state.encoder_count_ = this_status->encoder_count_;
00634   state.position_ = double(this_status->encoder_count_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI - state.zero_offset_;
00635   
00636   state.encoder_velocity_ = 
00637     calcEncoderVelocity(this_status->encoder_count_, this_status->timestamp_,
00638                         prev_status->encoder_count_, prev_status->timestamp_);
00639   state.velocity_ = state.encoder_velocity_ / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00640 
00641   state.calibration_reading_ = this_status->calibration_reading_ & LIMIT_SENSOR_0_STATE;
00642   state.calibration_rising_edge_valid_ = this_status->calibration_reading_ &  LIMIT_OFF_TO_ON;
00643   state.calibration_falling_edge_valid_ = this_status->calibration_reading_ &  LIMIT_ON_TO_OFF;
00644   state.last_calibration_rising_edge_ = double(this_status->last_calibration_rising_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00645   state.last_calibration_falling_edge_ = double(this_status->last_calibration_falling_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00646   state.is_enabled_ = bool(this_status->mode_ & MODE_ENABLE);
00647 
00648   state.last_executed_current_ = this_status->programmed_current_ * config_info_.nominal_current_scale_;
00649   state.last_measured_current_ = this_status->measured_current_ * config_info_.nominal_current_scale_;
00650 
00651   state.last_executed_effort_ = this_status->programmed_current_ * config_info_.nominal_current_scale_ * actuator_info_.motor_torque_constant_ * actuator_info_.encoder_reduction_;
00652   state.last_measured_effort_ = this_status->measured_current_ * config_info_.nominal_current_scale_ * actuator_info_.motor_torque_constant_ * actuator_info_.encoder_reduction_;
00653 
00654   state.num_encoder_errors_ = this_status->num_encoder_errors_;
00655 
00656   state.motor_voltage_ = this_status->motor_voltage_ * config_info_.nominal_voltage_scale_;
00657 
00658   state.max_effort_ = max_current_ * actuator_info_.encoder_reduction_ * actuator_info_.motor_torque_constant_; 
00659 
00660   return verifyState(this_status, prev_status);
00661 }
00662 
00663 
00664 bool WG0X::verifyChecksum(const void* buffer, unsigned size)
00665 {
00666   bool success = wg_util::computeChecksum(buffer, size) == 0;
00667   if (!success) {
00668     if (tryLockWG0XDiagnostics()) {
00669       ++wg0x_collect_diagnostics_.checksum_errors_;
00670       unlockWG0XDiagnostics();
00671     }
00672   }
00673   return success;
00674 }
00675 
00676 
00677 
00684 int32_t WG0X::timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp)
00685 {
00686   return int32_t(new_timestamp - old_timestamp);
00687 }
00688 
00695 ros::Duration WG0X::timediffToDuration(int32_t timediff_usec)
00696 {
00697   static const int USEC_PER_SEC = 1000000;
00698   int sec  = timediff_usec / USEC_PER_SEC;
00699   int nsec = (timediff_usec % USEC_PER_SEC)*1000;
00700   return ros::Duration(sec,nsec);
00701 }
00702 
00703 
00709 int32_t WG0X::positionDiff(int32_t new_position, int32_t old_position)
00710 {
00711   return int32_t(new_position - old_position);
00712 }
00713 
00720 double WG0X::calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp, 
00721                                  int32_t old_position, uint32_t old_timestamp)
00722 {
00723   double timestamp_diff = double(timestampDiff(new_timestamp, old_timestamp)) * 1e-6;
00724   double position_diff = double(positionDiff(new_position, old_position));
00725   return (position_diff / timestamp_diff);
00726 }
00727 
00728 
00735 double WG0X::convertRawTemperature(int16_t raw_temp)
00736 {
00737   return 0.0078125 * double(raw_temp);
00738 }
00739 
00740 
00741 // Returns true if timestamp changed by more than (amount) or time goes in reverse.
00742 bool WG0X::timestamp_jump(uint32_t timestamp, uint32_t last_timestamp, uint32_t amount)
00743 {
00744   uint32_t timestamp_diff = (timestamp - last_timestamp);
00745   return (timestamp_diff > amount);
00746 }
00747 
00748 bool WG0X::verifyState(WG0XStatus *this_status, WG0XStatus *prev_status)
00749 {
00750   pr2_hardware_interface::ActuatorState &state = actuator_.state_;
00751   bool rv = true;
00752 
00753   if ((motor_model_ != NULL) || (motor_heating_model_ != NULL))
00754   {
00755     // Both motor model and motor heating model use MotorTraceSample
00756     ethercat_hardware::MotorTraceSample &s(motor_trace_sample_);
00757     double last_executed_current =  this_status->programmed_current_ * config_info_.nominal_current_scale_;
00758     double supply_voltage = double(prev_status->supply_voltage_) * config_info_.nominal_voltage_scale_;
00759     double pwm_ratio = double(this_status->programmed_pwm_value_) / double(PWM_MAX);
00760     s.timestamp        = state.timestamp_;
00761     s.enabled          = state.is_enabled_;
00762     s.supply_voltage   = supply_voltage;
00763     s.measured_motor_voltage = state.motor_voltage_;
00764     s.programmed_pwm   = pwm_ratio;
00765     s.executed_current = last_executed_current;
00766     s.measured_current = state.last_measured_current_;
00767     s.velocity         = state.velocity_;
00768     s.encoder_position = state.position_;
00769     s.encoder_error_count = state.num_encoder_errors_;
00770 
00771     if (motor_model_ != NULL)
00772     {
00773       // Collect data for motor model
00774       motor_model_->sample(s);
00775       motor_model_->checkPublish();
00776     }
00777     if (motor_heating_model_ != NULL)
00778     {
00779       double ambient_temperature = convertRawTemperature(this_status->board_temperature_);
00780       double duration = double(timestampDiff(this_status->timestamp_, prev_status->timestamp_)) * 1e-6;
00781       motor_heating_model_->update(s, actuator_info_msg_, ambient_temperature, duration);
00782 
00783       if ((!motor_heating_model_common_->disable_halt_) && (motor_heating_model_->hasOverheated()))
00784       {
00785         rv = false;
00786       }
00787     }
00788   }
00789 
00790   max_board_temperature_ = max(max_board_temperature_, this_status->board_temperature_);
00791   max_bridge_temperature_ = max(max_bridge_temperature_, this_status->bridge_temperature_);
00792 
00793   if (this_status->timestamp_ == last_timestamp_ ||
00794       this_status->timestamp_ == last_last_timestamp_) {
00795     ++drops_;
00796     ++consecutive_drops_;
00797     max_consecutive_drops_ = max(max_consecutive_drops_, consecutive_drops_);
00798   } else {
00799     consecutive_drops_ = 0;
00800   }
00801   // Detect timestamps going in reverse or changing by more than 10 seconds = 10,000,000 usec
00802   if ( timestamp_jump(this_status->timestamp_,last_timestamp_,10000000) )
00803   {
00804     timestamp_jump_detected_ = true;
00805   }
00806   last_last_timestamp_ = last_timestamp_;
00807   last_timestamp_ = this_status->timestamp_;
00808 
00809   if (consecutive_drops_ > 10)
00810   {
00811     too_many_dropped_packets_ = true;
00812     rv = false;
00813     goto end;
00814   }
00815 
00816   in_lockout_ = bool(this_status->mode_ & MODE_SAFETY_LOCKOUT);
00817   if (in_lockout_ && !resetting_)
00818   {
00819     rv = false;
00820     goto end;
00821   }
00822 
00823   if (fpga_internal_reset_detected_)
00824   {
00825     rv = false;
00826     goto end;
00827   }
00828 
00829   if (this_status->num_encoder_errors_ != prev_status->num_encoder_errors_)
00830   {
00831     encoder_errors_detected_ = true;
00832   }
00833 
00834   if (state.is_enabled_ && motor_model_)
00835   {
00836     if (!disable_motor_model_checking_)
00837     {
00838       if(!motor_model_->verify())
00839       {
00840         // Motor model will automatically publish a motor trace when there is an error
00841         rv = false;
00842         goto end;
00843       }
00844     }
00845   }
00846 
00847 end:
00848   if (motor_model_) 
00849   {
00850     // Publish trace when:
00851     //  * device goes into safety lockout
00852     //  * controller request motor trace to be published
00853     bool new_error = in_lockout_ && !resetting_ && !has_error_;
00854     if (new_error || publish_motor_trace_.command_.data_)
00855     {
00856       const char* reason = "Publishing manually triggered";
00857       if (new_error)
00858       {
00859         bool undervoltage = (this_status->mode_ & MODE_UNDERVOLTAGE);    
00860         reason = (undervoltage) ? "Undervoltage Lockout" : "Safety Lockout";
00861       }    
00862       int level          = (new_error) ? 2 : 0;
00863       motor_model_->flagPublish(reason, level , 100);
00864       publish_motor_trace_.command_.data_ = 0;
00865     }
00866   }
00867   bool is_error = !rv;
00868   has_error_ = is_error || has_error_;
00869   actuator_.state_.halted_ = has_error_ || this_status->mode_ == MODE_OFF;
00870   return rv;
00871 }
00872 
00873 bool WG0X::publishTrace(const string &reason, unsigned level, unsigned delay)
00874 {
00875   if (motor_model_) 
00876   {
00877     motor_model_->flagPublish(reason, level, delay);
00878     return true;
00879   }
00880   return false;
00881 }
00882 
00883 
00884 void WG0X::collectDiagnostics(EthercatCom *com)
00885 {
00886   //Collect safety disable information through mailbox  
00887   bool success = false;
00888 
00889   // Have parent collect diagnositcs
00890   EthercatDevice::collectDiagnostics(com);
00891 
00892   // Send a packet with both a Fixed address read (NPRW) to device to make sure it is present in chain.
00893   // This avoids wasting time trying to read mailbox of device that it not present on chain.
00894   {
00895     EC_Logic *logic = EC_Logic::instance();
00896     unsigned char buf[1];
00897     EC_UINT address = 0x0000;
00898     NPRD_Telegram nprd_telegram(logic->get_idx(),
00899                                 sh_->get_station_address(),
00900                                 address,
00901                                 0 /*working counter*/,
00902                                 sizeof(buf),
00903                                 buf);
00904     EC_Ethernet_Frame frame(&nprd_telegram);
00905     if (!com->txandrx_once(&frame)) {
00906       // packet didn't come back
00907       goto end;
00908     }
00909     if (nprd_telegram.get_wkc() != 1) {
00910       // packet came back, but device didn't not respond
00911       goto end;
00912     }
00913   }
00914  
00915   WG0XSafetyDisableStatus s;
00916   if (readMailbox(com, s.BASE_ADDR, &s, sizeof(s)) != 0) {
00917     goto end;
00918   }
00919     
00920   WG0XDiagnosticsInfo di;
00921   if (readMailbox(com, di.BASE_ADDR, &di, sizeof(di)) != 0) {
00922     goto end;
00923   }
00924   
00925   { // Try writing zero offset to to WG0X devices that have application ram
00926     WG0XDiagnostics &dg(wg0x_collect_diagnostics_);
00927 
00928     if ((app_ram_status_ == APP_RAM_PRESENT) && (dg.zero_offset_ != dg.cached_zero_offset_))
00929     {
00930       if (writeAppRam(com, dg.zero_offset_)){
00931         ROS_DEBUG("Writing new calibration to device %s, new %f, old %f", actuator_info_.name_, dg.zero_offset_, dg.cached_zero_offset_);
00932         dg.cached_zero_offset_ = dg.zero_offset_;
00933       }
00934       else{
00935         ROS_ERROR("Failed to write new calibration to device %s, new %f, old %f", actuator_info_.name_, dg.zero_offset_, dg.cached_zero_offset_);
00936         // Diagnostics thread will try again next update cycle
00937       }
00938     }
00939   }
00940 
00941   success = true;
00942 
00943  end:
00944   if (!lockWG0XDiagnostics()) {
00945     wg0x_collect_diagnostics_.valid_ = false;   // change these values even if we did't get the lock
00946     wg0x_collect_diagnostics_.first_ = false;   
00947     return;
00948   }
00949 
00950   wg0x_collect_diagnostics_.valid_ = success;   
00951   if (success) {
00952     wg0x_collect_diagnostics_.update(s,di);
00953   }
00954 
00955   unlockWG0XDiagnostics();
00956 }
00957 
00958 
00959 bool WG0X::writeAppRam(EthercatCom *com, double zero_offset) 
00960 {
00961   WG0XUserConfigRam cfg;
00962   cfg.version_ = 1;
00963   cfg.zero_offset_ = zero_offset;
00964   boost::crc_32_type crc32;
00965   crc32.process_bytes(&cfg, sizeof(cfg)-sizeof(cfg.crc32_));
00966   cfg.crc32_ = crc32.checksum();
00967   return (writeMailbox(com, WG0XUserConfigRam::BASE_ADDR, &cfg, sizeof(cfg)) == 0);
00968 }
00969 
00970 bool WG0X::readAppRam(EthercatCom *com, double &zero_offset) 
00971 {
00972   WG0XUserConfigRam cfg;
00973   if (!readMailbox(com, WG0XUserConfigRam::BASE_ADDR, &cfg, sizeof(cfg)) == 0)
00974   {
00975     return false;
00976   }
00977   if (cfg.version_ != 1) 
00978   {
00979     return false;
00980   }
00981   boost::crc_32_type crc32;
00982   crc32.process_bytes(&cfg, sizeof(cfg)-sizeof(cfg.crc32_));
00983   if (cfg.crc32_ != crc32.checksum()) {
00984     return false;
00985   }
00986   zero_offset = cfg.zero_offset_;
00987   return true;
00988 }
00989 
00990 
00998 bool WG0X::readActuatorInfoFromEeprom(EthercatCom *com, WG0XActuatorInfo &actuator_info)
00999 {
01000   BOOST_STATIC_ASSERT(sizeof(actuator_info) == 264);
01001 
01002   if (!eeprom_.readEepromPage(com, &mailbox_, ACTUATOR_INFO_PAGE, &actuator_info, sizeof(actuator_info)))
01003   {
01004     ROS_ERROR("Reading acutuator info from eeprom");
01005     return false;
01006   }
01007   return true;
01008 }
01009  
01017 bool WG0X::readMotorHeatingModelParametersFromEeprom(EthercatCom *com, MotorHeatingModelParametersEepromConfig &config)
01018 {
01019   BOOST_STATIC_ASSERT(sizeof(config) == 256);
01020 
01021   if (!eeprom_.readEepromPage(com, &mailbox_, config.EEPROM_PAGE, &config, sizeof(config)))
01022   {
01023     ROS_ERROR("Reading motor heating model config from eeprom");
01024     return false;
01025   }
01026   return true;
01027 }
01028 
01029 
01030 
01031 
01050 bool WG0X::program(EthercatCom *com, const WG0XActuatorInfo &actutor_info)
01051 {
01052   if (!eeprom_.writeEepromPage(com, &mailbox_, ACTUATOR_INFO_PAGE, &actutor_info, sizeof(actutor_info)))
01053   {
01054     ROS_ERROR("Writing actuator infomation to EEPROM");
01055     return false;
01056   }
01057   
01058   return true;
01059 }
01060 
01061 
01076 bool WG0X::program(EthercatCom *com, const ethercat_hardware::MotorHeatingModelParametersEepromConfig &heating_config)
01077 {
01078   if (!eeprom_.writeEepromPage(com, &mailbox_, heating_config.EEPROM_PAGE, &heating_config, sizeof(heating_config)))
01079   {
01080     ROS_ERROR("Writing motor heating model configuration to EEPROM");
01081     return false;
01082   }
01083   
01084   return true;  
01085 }
01086 
01087 
01101 int WG0X::readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length)
01102 {
01103   return mailbox_.readMailbox(com, address, data, length);
01104 }
01105 
01106 bool WG0X::lockWG0XDiagnostics() 
01107 {
01108   int error = pthread_mutex_lock(&wg0x_diagnostics_lock_);
01109   if (error != 0) {
01110     fprintf(stderr, "%s : " ERROR_HDR " getting diagnostics lock\n", __func__);
01111     // update error counters even if we didn't get lock
01112     ++wg0x_collect_diagnostics_.lock_errors_;
01113     return false;
01114   }
01115   return true;
01116 }
01117 
01118 bool WG0X::tryLockWG0XDiagnostics() 
01119 {
01120   int error = pthread_mutex_trylock(&wg0x_diagnostics_lock_);
01121   if (error == EBUSY) {
01122     return false;
01123   }
01124   else if (error != 0) {
01125     fprintf(stderr, "%s : " ERROR_HDR " getting diagnostics lock\n", __func__);
01126     // update error counters even if we didn't get lock
01127     ++wg0x_collect_diagnostics_.lock_errors_;
01128     return false;
01129   }
01130   return true;
01131 }
01132 
01133 void WG0X::unlockWG0XDiagnostics() 
01134 {
01135   int error = pthread_mutex_unlock(&wg0x_diagnostics_lock_);
01136   if (error != 0) {
01137     fprintf(stderr, "%s : " ERROR_HDR " freeing diagnostics lock\n", __func__);
01138     ++wg0x_collect_diagnostics_.lock_errors_;
01139   }
01140 }
01141 
01142 
01155 int WG0X::writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length)
01156 {
01157   return mailbox_.writeMailbox(com,address,data,length);
01158 }
01159 
01160 
01161 
01162 #define CHECK_SAFETY_BIT(bit) \
01163   do { if (status & SAFETY_##bit) { \
01164     str += prefix + #bit; \
01165     prefix = ", "; \
01166   } } while(0)
01167 
01168 string WG0X::safetyDisableString(uint8_t status)
01169 {
01170   string str, prefix;
01171 
01172   if (status & SAFETY_DISABLED)
01173   {
01174     CHECK_SAFETY_BIT(DISABLED);
01175     CHECK_SAFETY_BIT(UNDERVOLTAGE);
01176     CHECK_SAFETY_BIT(OVER_CURRENT);
01177     CHECK_SAFETY_BIT(BOARD_OVER_TEMP);
01178     CHECK_SAFETY_BIT(HBRIDGE_OVER_TEMP);
01179     CHECK_SAFETY_BIT(OPERATIONAL);
01180     CHECK_SAFETY_BIT(WATCHDOG);
01181   }
01182   else
01183     str = "ENABLED";
01184 
01185   return str;
01186 }
01187 
01188 string WG0X::modeString(uint8_t mode)
01189 {
01190   string str, prefix;
01191   if (mode) {
01192     if (mode & MODE_ENABLE) {
01193       str += prefix + "ENABLE";
01194       prefix = ", ";
01195     }
01196     if (mode & MODE_CURRENT) {
01197       str += prefix + "CURRENT";
01198       prefix = ", ";
01199     }
01200     if (mode & MODE_UNDERVOLTAGE) {
01201       str += prefix + "UNDERVOLTAGE";
01202       prefix = ", ";
01203     }
01204     if (mode & MODE_SAFETY_RESET) {
01205       str += prefix + "SAFETY_RESET";
01206       prefix = ", ";
01207     }
01208     if (mode & MODE_SAFETY_LOCKOUT) {
01209       str += prefix + "SAFETY_LOCKOUT";
01210       prefix = ", ";
01211     }
01212     if (mode & MODE_RESET) {
01213       str += prefix + "RESET";
01214       prefix = ", ";
01215     }
01216   } else {
01217     str = "OFF";
01218   }
01219   return str;
01220 }
01221 
01222 void WG0X::publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
01223 { 
01224   // If possible, copy new diagnositics from collection thread, into diagnostics thread
01225   if (tryLockWG0XDiagnostics()) { 
01226     wg0x_publish_diagnostics_ = wg0x_collect_diagnostics_;
01227     unlockWG0XDiagnostics(); 
01228   }
01229 
01230   if (too_many_dropped_packets_)
01231   {
01232     d.mergeSummary(d.ERROR, "Too many dropped packets");
01233   }
01234 
01235   if (status_checksum_error_)
01236   {
01237     d.mergeSummary(d.ERROR, "Checksum error on status data");
01238   }
01239   
01240   if (wg0x_publish_diagnostics_.first_)
01241   {
01242     d.mergeSummary(d.WARN, "Have not yet collected WG0X diagnostics");
01243   }
01244   else if (!wg0x_publish_diagnostics_.valid_) 
01245   {
01246     d.mergeSummary(d.WARN, "Could not collect WG0X diagnostics");
01247   }
01248 
01249   WG0XDiagnostics const &p(wg0x_publish_diagnostics_);
01250   WG0XSafetyDisableStatus const &s(p.safety_disable_status_);
01251   d.addf("Status Checksum Error Count", "%d", p.checksum_errors_);
01252   d.addf("Safety Disable Status", "%s (%02x)", safetyDisableString(s.safety_disable_status_).c_str(), s.safety_disable_status_);
01253   d.addf("Safety Disable Status Hold", "%s (%02x)", safetyDisableString(s.safety_disable_status_hold_).c_str(), s.safety_disable_status_hold_);
01254   d.addf("Safety Disable Count", "%d", p.safety_disable_total_);
01255   d.addf("Undervoltage Count", "%d", p.undervoltage_total_);
01256   d.addf("Over Current Count", "%d", p.over_current_total_);
01257   d.addf("Board Over Temp Count", "%d", p.board_over_temp_total_);
01258   d.addf("Bridge Over Temp Count", "%d", p.bridge_over_temp_total_);
01259   d.addf("Operate Disable Count", "%d", p.operate_disable_total_);
01260   d.addf("Watchdog Disable Count", "%d", p.watchdog_disable_total_);
01261 
01262   if (in_lockout_)
01263   {
01264     uint8_t status = s.safety_disable_status_hold_;
01265     string prefix(": "); 
01266     string str("Safety Lockout");
01267     CHECK_SAFETY_BIT(UNDERVOLTAGE);
01268     CHECK_SAFETY_BIT(OVER_CURRENT);
01269     CHECK_SAFETY_BIT(BOARD_OVER_TEMP);
01270     CHECK_SAFETY_BIT(HBRIDGE_OVER_TEMP);
01271     CHECK_SAFETY_BIT(OPERATIONAL);
01272     CHECK_SAFETY_BIT(WATCHDOG);
01273     d.mergeSummary(d.ERROR, str);
01274   }
01275 
01276   if (timestamp_jump_detected_ && (s.safety_disable_status_hold_ & SAFETY_OPERATIONAL))
01277   {
01278     fpga_internal_reset_detected_ = true;
01279   }
01280 
01281   if (fpga_internal_reset_detected_) 
01282   {
01283     d.mergeSummaryf(d.ERROR, "FPGA internal reset detected");
01284   }
01285   
01286   if (timestamp_jump_detected_)
01287   {
01288     d.mergeSummaryf(d.WARN, "Timestamp jumped");
01289   }
01290 
01291   {
01292 
01293     const WG0XDiagnosticsInfo &di(p.diagnostics_info_);
01294     //d.addf("PDO Command IRQ Count", "%d", di.pdo_command_irq_count_);
01295     d.addf("MBX Command IRQ Count", "%d", di.mbx_command_irq_count_);
01296     d.addf("PDI Timeout Error Count", "%d", di.pdi_timeout_error_count_);
01297     d.addf("PDI Checksum Error Count", "%d", di.pdi_checksum_error_count_);
01298     unsigned product = sh_->get_product_code();
01299 
01300     // Current scale 
01301     if ((product == WG05_PRODUCT_CODE) && (board_major_ == 1))
01302     {
01303       // WG005B measure current going into and out-of H-bridge (not entire board)
01304       static const double WG005B_SUPPLY_CURRENT_SCALE = (1.0 / (8152.0 * 0.851)) * 4.0;
01305       double bridge_supply_current = double(di.supply_current_in_) * WG005B_SUPPLY_CURRENT_SCALE;
01306       d.addf("Bridge Supply Current", "%f", bridge_supply_current);
01307     }
01308     else if ((product == WG05_PRODUCT_CODE) || (product == WG021_PRODUCT_CODE)) 
01309     {
01310       // WG005[CDEF] measures curret going into entire board.  It cannot measure negative (regenerative) current values.
01311       // WG021A == WG005E,  WG021B == WG005F
01312       static const double WG005_SUPPLY_CURRENT_SCALE = ((82.0 * 2.5) / (0.01 * 5100.0 * 32768.0));
01313       double supply_current = double(di.supply_current_in_) * WG005_SUPPLY_CURRENT_SCALE;
01314       d.addf("Supply Current", "%f",  supply_current);
01315     }
01316     d.addf("Configured Offset A", "%f", config_info_.nominal_current_scale_ * di.config_offset_current_A_);
01317     d.addf("Configured Offset B", "%f", config_info_.nominal_current_scale_ * di.config_offset_current_B_);
01318   }
01319 }
01320 
01321 
01322 
01323 void WG0X::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
01324 {
01325   WG0XStatus *status = (WG0XStatus *)(buffer + command_size_);
01326 
01327   stringstream str;
01328   str << "EtherCAT Device (" << actuator_info_.name_ << ")";
01329   d.name = str.str();
01330   char serial[32];
01331   snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
01332   d.hardware_id = serial;
01333 
01334   d.summary(d.OK, "OK");
01335 
01336   d.clear();
01337   d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
01338   d.add("Name", actuator_info_.name_);
01339   d.addf("Position", "%02d", sh_->get_ring_position());
01340   d.addf("Product code",
01341         "WG0%d (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
01342         sh_->get_product_code() == WG05_PRODUCT_CODE ? 5 : 6,
01343         sh_->get_product_code(), fw_major_, fw_minor_,
01344         'A' + board_major_, board_minor_);
01345 
01346   d.add("Robot", actuator_info_.robot_name_);
01347   d.addf("Motor", "%s %s", actuator_info_.motor_make_, actuator_info_.motor_model_);
01348   d.add("Serial Number", serial);
01349   d.addf("Nominal Current Scale", "%f",  config_info_.nominal_current_scale_);
01350   d.addf("Nominal Voltage Scale",  "%f", config_info_.nominal_voltage_scale_);
01351   d.addf("HW Max Current", "%f", config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_);
01352 
01353   d.addf("SW Max Current", "%f", actuator_info_.max_current_);
01354   d.addf("Speed Constant", "%f", actuator_info_.speed_constant_);
01355   d.addf("Resistance", "%f", actuator_info_.resistance_);
01356   d.addf("Motor Torque Constant", "%f", actuator_info_.motor_torque_constant_);
01357   d.addf("Pulses Per Revolution", "%d", actuator_info_.pulses_per_revolution_);
01358   d.addf("Encoder Reduction", "%f", actuator_info_.encoder_reduction_);
01359 
01360   publishGeneralDiagnostics(d);
01361   mailbox_.publishMailboxDiagnostics(d);
01362 
01363   d.addf("Calibration Offset", "%f", cached_zero_offset_);
01364   d.addf("Calibration Status", "%s", 
01365          (calibration_status_ == NO_CALIBRATION) ? "No calibration" :
01366          (calibration_status_ == CONTROLLER_CALIBRATION) ? "Calibrated by controller" :
01367          (calibration_status_ == SAVED_CALIBRATION) ? "Using saved calibration" : "UNKNOWN");
01368 
01369   d.addf("Watchdog Limit", "%dms", config_info_.watchdog_limit_);
01370   d.add("Mode", modeString(status->mode_));
01371   d.addf("Digital out", "%d", status->digital_out_);
01372   d.addf("Programmed pwm value", "%d", status->programmed_pwm_value_);
01373   d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
01374   d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
01375   d.addf("Timestamp", "%u", status->timestamp_);
01376   d.addf("Encoder count", "%d", status->encoder_count_);
01377   d.addf("Encoder index pos", "%d", status->encoder_index_pos_);
01378   d.addf("Num encoder_errors", "%d", status->num_encoder_errors_);
01379   d.addf("Encoder status", "%d", status->encoder_status_);
01380   d.addf("Calibration reading", "%d", status->calibration_reading_);
01381   d.addf("Last calibration rising edge", "%d", status->last_calibration_rising_edge_);
01382   d.addf("Last calibration falling edge", "%d", status->last_calibration_falling_edge_);
01383   d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
01384   d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
01385   d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
01386   d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
01387   d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
01388   d.addf("Motor voltage", "%f", status->motor_voltage_ * config_info_.nominal_voltage_scale_);
01389   d.addf("Current Loop Kp", "%d", config_info_.current_loop_kp_);
01390   d.addf("Current Loop Ki", "%d", config_info_.current_loop_ki_);
01391 
01392   if (motor_model_) 
01393   {
01394     motor_model_->diagnostics(d);
01395     if (disable_motor_model_checking_)
01396     {
01397       d.mergeSummaryf(d.WARN, "Motor model disabled");      
01398     }
01399   }
01400 
01401   if (motor_heating_model_.get() != NULL)
01402   {
01403     motor_heating_model_->diagnostics(d);
01404   }
01405 
01406   if (encoder_errors_detected_)
01407   {
01408     d.mergeSummaryf(d.WARN, "Encoder errors detected");
01409   }
01410 
01411   d.addf("Packet count", "%d", status->packet_count_);
01412 
01413   d.addf("Drops", "%d", drops_);
01414   d.addf("Consecutive Drops", "%d", consecutive_drops_);
01415   d.addf("Max Consecutive Drops", "%d", max_consecutive_drops_);
01416 
01417   unsigned numPorts = (sh_->get_product_code()==WG06_PRODUCT_CODE) ? 1 : 2; // WG006 has 1 port, WG005 has 2
01418   EthercatDevice::ethercatDiagnostics(d, numPorts); 
01419 }
01420 


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:45