EMCY.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK Canopen Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 // -- END LICENSE BLOCK ------------------------------------------------
00014 
00015 //----------------------------------------------------------------------
00023 //----------------------------------------------------------------------
00024 
00025 #include "EMCY.h"
00026 #include "ds301.h"
00027 
00028 #include "Logging.h"
00029 #include "exceptions.h"
00030 
00031 namespace icl_hardware {
00032 namespace canopen_schunk {
00033 
00034 // static initializers of error_maps
00035 std::map <uint16_t, std::string> EMCY::m_eec_map;
00036 std::map <uint8_t, std::string> EMCY::m_error_register_map;
00037 
00038 EMCY::EMCY(const uint8_t node_id)
00039   : m_node_id(node_id)
00040 {
00041 }
00042 
00043 void EMCY::update(const CanMsg& msg)
00044 {
00045   uint8_t node_id = msg.id - ds301::ID_EMCY_MIN + 1;
00046 
00047   if (node_id != m_node_id)
00048   {
00049     LOGGING_ERROR_C(CanOpen, EMCY, "EMCY Update called with wrong canopen ID. Received ID: "
00050                                    << node_id << " Node ID: " << m_node_id
00051                                    << ". Update ignored." << endl);
00052     return;
00053   }
00054 
00055 
00056   if (msg.dlc != 8)
00057   {
00058     std::stringstream ss;
00059     ss << "Unexpected length " << static_cast<int>(msg.dlc) << " of EMCY message. Expected 8.";
00060     LOGGING_ERROR_C (CanOpen, SDO, ss.str() << endl);
00061     return;
00062   }
00063 
00064   boost::mutex::scoped_lock(m_data_buffer_mutex);
00065   // isolate information from message
00066   m_eec = msg.data[0] + (msg.data[1] << 8);
00067   m_error_register = msg.data[2];
00068   m_msef.resize(5);
00069   for (size_t i = 3; i < 8; ++i)
00070   {
00071     m_msef[i-3] = msg.data[i];
00072   }
00073 
00074   if (m_eec == EMCY_ERROR_RESET_NO_ERROR)
00075   {
00076     m_error_state = EMCY_STATE_ERROR_FREE;
00077     LOGGING_INFO_C(CanOpen, EMCY, "Error reset EMCY received. Node " << m_node_id
00078     << " is now in state error free." << endl);
00079   }
00080   else
00081   {
00082     m_error_state = canopen_schunk::EMCY::EMCY_STATE_ERROR_OCCURED;
00083     std::stringstream ss;
00084     ss << "EMCY message states that an error in node "
00085        << static_cast<int>(m_node_id) << " occured: "
00086        << lookupEECString(m_eec) << std::endl
00087        << "Error registers: " << lookupErrorRegisterString(m_error_register) << std::endl
00088        << "Manufacturer specific code: " << lookupMSEFString();
00089     LOGGING_ERROR_C(CanOpen, EMCY, ss.str() << endl);
00090   }
00091 }
00092 
00093 EMCY::eEMCY_STATUS EMCY::getEmcyStatus() const
00094 {
00095   boost::mutex::scoped_lock(m_data_buffer_mutex);
00096   return m_error_state;
00097 }
00098 
00099 bool EMCY::getErrorInformation(uint16_t& eec, uint8_t& error_register, std::vector< uint8_t >& msef)
00100 {
00101   boost::mutex::scoped_lock(m_data_buffer_mutex);
00102 
00103   // No error occured, so we can give no valid error information
00104   if (m_error_state == EMCY_STATE_ERROR_FREE)
00105   {
00106     return false;
00107   }
00108 
00109   eec = m_eec;
00110   error_register = m_error_register;
00111   msef = m_msef;
00112   return true;
00113 }
00114 
00115 std::string EMCY::lookupEECString(const uint16_t error_code)
00116 {
00117   std::stringstream ss;
00118   ss << std::endl;
00119   uint16_t nibble_validator = 0xF000;
00120   uint16_t subcode = error_code & nibble_validator;
00121   std::map<uint16_t, std::string>::iterator map_it = m_eec_map.find(subcode);
00122   if (map_it != m_eec_map.end())
00123   {
00124     ss << map_it->second << std::endl;
00125     uint16_t nibble_validator_next = nibble_validator >> 4;
00126     uint16_t next_nibble = error_code & nibble_validator_next;
00127     ++map_it;
00128 
00129     // DS402 allows additional functions to be defined directly as 0xF00N
00130     // (according to table 3 in CiA DSP-402 V1.1)
00131     while (next_nibble == 0 && nibble_validator_next > 0)
00132     {
00133       subcode |= next_nibble;
00134       nibble_validator += nibble_validator_next;
00135       nibble_validator_next = nibble_validator_next >> 4;
00136       next_nibble = error_code & nibble_validator_next;
00137     }
00138 
00139     // Iterate forward in the map (it is sorted) as long as the code matches
00140     while ((map_it->first & nibble_validator) == subcode)
00141     {
00142       if ( (map_it->first & nibble_validator_next) == next_nibble )
00143       {
00144         // found the next nibble. Move everything forward one nibble.
00145         while ((map_it->first & nibble_validator_next) != 0 && subcode != error_code)
00146         {
00147           subcode |= next_nibble;
00148           nibble_validator += nibble_validator_next;
00149           nibble_validator_next = nibble_validator_next >> 4;
00150           next_nibble = error_code & nibble_validator_next;
00151         }
00152 
00153         if ( (map_it->first - subcode) == 0 )
00154         {
00155           // add value to output string
00156           ss << map_it->second << std::endl;
00157         }
00158       }
00159       ++map_it;
00160     }
00161 
00162     // If the error code was more specific than the most specific entry found, we inform the user about that.
00163     if (subcode != error_code)
00164     {
00165       ss << "Error code could not be fully parsed. Full error code was " << hexToString(error_code) << std::endl;
00166     }
00167 
00168     return ss.str();
00169   }
00170   else
00171   {
00172     std::stringstream ss;
00173     ss << "Unknown emergency error code: " << hexToString(error_code);
00174     return ss.str();
00175   }
00176 }
00177 
00178 std::string EMCY::lookupErrorRegisterString ( const uint8_t error_code )
00179 {
00180   std::stringstream ss;
00181 
00182   if (m_error_register_map.size() == 0)
00183   {
00184     ss << "No error register description set. Returning plain register code: "
00185        << hexToString(error_code) << std::endl;
00186   }
00187   else
00188   {
00189     for (size_t i = 0; i < (sizeof(error_code) * 8); ++i)
00190     {
00191       uint8_t bit = (0x01 << i) & error_code;
00192       std::map<uint8_t, std::string>::iterator map_it = m_error_register_map.find(bit);
00193       if (map_it != m_error_register_map.end())
00194       {
00195         ss << map_it->second << ", ";
00196       }
00197     }
00198   }
00199 
00200   if (ss.str().empty())
00201   {
00202     ss << "Unknown error register code: " << hexToString(error_code);
00203   }
00204   return ss.str();
00205 }
00206 
00207 
00208 std::string EMCY::lookupMSEFString() const
00209 {
00210   return hexArrayToString(&m_msef[0], m_msef.size());
00211 }
00212 
00213 void EMCY::addEmergencyErrorMap(const std::string& filename, const std::string& block_identifier)
00214 {
00215   std::map<uint32_t, std::string> new_entries = getErrorMapFromConfigFile(filename, block_identifier);
00216   for (std::map<uint32_t, std::string>::iterator it = new_entries.begin();
00217        it != new_entries.end();
00218        ++it)
00219   {
00220     uint16_t index = static_cast<uint16_t>(it->first);
00221     m_eec_map[index] = it->second;
00222   }
00223 }
00224 
00225 void EMCY::addErrorRegisterMap ( const std::string& filename, const std::string& block_identifier )
00226 {
00227   std::map<uint32_t, std::string> new_entries = getErrorMapFromConfigFile(filename, block_identifier);
00228   for (std::map<uint32_t, std::string>::iterator it = new_entries.begin();
00229        it != new_entries.end();
00230        ++it)
00231   {
00232     uint8_t index = static_cast<uint8_t>(it->first);
00233     m_error_register_map[index] = it->second;
00234   }
00235 }
00236 
00237 void EMCY::printError (SDO& sdo, const uint8_t error_nr)
00238 {
00239   std::vector<uint8_t> uploaded_data;
00240   sdo.upload(false, 0x1003, error_nr, uploaded_data);
00241 
00242   if (uploaded_data.size() != 4) // should be uint32
00243   {
00244     throw ProtocolException (0x1003, error_nr, "Uploaded data size does not match 4");
00245   }
00246 
00247   uint16_t eec = uploaded_data[0] + (uploaded_data[1] << 8);
00248   uint16_t additional_information = uploaded_data[2] + (uploaded_data[3] << 8);
00249 
00250   std::stringstream ss;
00251   ss << " Error " << static_cast<int>(error_nr) << ": "
00252      << lookupEECString(eec) << std::endl
00253      << "Additional information: " << hexToString(additional_information);
00254   LOGGING_ERROR_C(CanOpen, EMCY, ss.str() << endl);
00255 }
00256 
00257 
00258 void EMCY::printLastErrors(SDO& sdo)
00259 {
00260   // first get the number of recorded errors
00261   uint8_t num_errors;
00262   sdo.upload(false, 0x1003, 0, num_errors);
00263 
00264   LOGGING_INFO_C (CanOpen, EMCY, num_errors << " errors in error history:" << endl);
00265 
00266   // get all present errors
00267   for (size_t i = 1; i <= num_errors; ++i)
00268   {
00269     printError (sdo, i);
00270   }
00271 
00272 }
00273 
00274 void EMCY::clearErrorHistory(SDO& sdo)
00275 {
00276   // write 0 to 0x1003/0
00277   uint8_t value = 0;
00278   sdo.download(false, 0x1003, 0, value);
00279 }
00280 
00281 
00282 
00283 
00284 }}//end of NS


schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Sun May 22 2016 03:30:56