DS301Node.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 "DS301Node.h"
00026 #include "exceptions.h"
00027 
00028 namespace icl_hardware {
00029 namespace canopen_schunk {
00030 
00031 DS301Node::DS301Node(const uint8_t node_id, const CanDevPtr& can_device, HeartBeatMonitor::Ptr heartbeat_monitor):
00032   m_nmt(node_id,can_device),
00033   m_sdo(node_id, can_device),
00034   m_emcy(EMCY::Ptr(new EMCY(node_id))),
00035   m_node_id(node_id),
00036   m_can_dev(can_device),
00037   m_heartbeat_monitor(heartbeat_monitor),
00038   m_heartbeat_cycle_time_ms(50)
00039 {
00040   for (size_t i = 0; i < 4; ++i)
00041   {
00042     m_rpdos.push_back(RPDO::Ptr(new RPDO(node_id, i, can_device)));
00043     m_tpdos.push_back(TPDO::Ptr(new TPDO(node_id, i, can_device)));
00044   }
00045 }
00046 
00047 void DS301Node::initNode()
00048 {
00049   m_nmt.preOperational();
00050   startHeartbeat();
00051 }
00052 
00053 void DS301Node::registerWSBroadcaster(boost::shared_ptr<icl_comm::websocket::WsBroadcaster> broadcaster)
00054 {
00055   m_ws_broadcaster = broadcaster;
00056 }
00057 
00058 void DS301Node::startHeartbeat()
00059 {
00060   m_sdo.download(false, 0x1017, 0, m_heartbeat_cycle_time_ms);
00061   // add initial heartbeat. If the device never sends a heartbeat the monitor will escalate quickly
00062   m_heartbeat_monitor->addHeartbeat(m_node_id);
00063 }
00064 
00065 
00066 void DS301Node::initPDOMappingSingle (const PDO::MappingConfigurationList& config,
00067                                       const uint16_t pdo_nr,
00068                                       const PDO::eTransmissionType& transmission_type,
00069                                       const ePDO_TYPE& direction_type,
00070                                       const bool dummy_mapping,
00071                                       const uint8_t cyclic_timeout_cycles)
00072 {
00073   PDO::PDOStringMatchVec mapping;
00074   boost::unordered_map<std::string, PDOMapEntry>* map;
00075   if (direction_type == RECEIVE_PDO)
00076   {
00077     mapping = m_rpdos.at(pdo_nr)->remap(m_sdo, config, transmission_type, dummy_mapping, cyclic_timeout_cycles);
00078     map = &m_rpdo_mapping;
00079   }
00080   else if (direction_type == TRANSMIT_PDO)
00081   {
00082     mapping = m_tpdos.at(pdo_nr)->remap(m_sdo, config, transmission_type, dummy_mapping, cyclic_timeout_cycles);
00083     map = &m_tpdo_mapping;
00084   }
00085   else
00086   {
00087     LOGGING_ERROR_C (CanOpen, DS301Node, "Illegal PDO type given. Only RECEIVE_PDO and TRANSMIT_PDO are allowed." << endl);
00088   }
00089 
00090   // remove all mapping entries from this pdo
00091   for (boost::unordered_map<std::string, PDOMapEntry>::iterator it = map->begin();
00092        it != map->end();
00093        /* iterating is done in the body */)
00094   {
00095     if (it->second.pdo_nr == pdo_nr)
00096     {
00097       map->erase(it++);
00098     }
00099     else
00100     {
00101       ++it;
00102     }
00103   }
00104 
00105   // Now add the configured pdos to the node's mapping
00106   for (PDO::PDOStringMatchVec::iterator it = mapping.begin(); it != mapping.end(); ++it)
00107   {
00108     PDOMapEntry entry;
00109     entry.pdo_nr = pdo_nr;
00110     entry.pdo_mapping_index = it->pdo_mapping_index;
00111 
00112     map->insert(std::pair<std::string, PDOMapEntry>(it->name, entry));
00113   }
00114 }
00115 
00116 void DS301Node::appendPDOMappingSingle (const PDO::MappingConfigurationList& config,
00117                                        const uint16_t pdo_nr,
00118                                        const PDO::eTransmissionType& transmission_type,
00119                                        const DS301Node::ePDO_TYPE& direction_type,
00120                                        const bool dummy_mapping,
00121                                        const uint8_t cyclic_timeout_cycles)
00122 {
00123   PDO::PDOStringMatchVec mapping;
00124   boost::unordered_map<std::string, PDOMapEntry>* map;
00125 
00126   if (direction_type == RECEIVE_PDO)
00127   {
00128     mapping = m_rpdos.at(pdo_nr)->appendMapping(m_sdo, config, transmission_type, dummy_mapping, cyclic_timeout_cycles);
00129     map = &m_rpdo_mapping;
00130   }
00131   else if (direction_type == TRANSMIT_PDO)
00132   {
00133     mapping = m_tpdos.at(pdo_nr)->appendMapping(m_sdo, config, transmission_type, dummy_mapping, cyclic_timeout_cycles);
00134     map = &m_tpdo_mapping;
00135   }
00136   else
00137   {
00138     LOGGING_ERROR_C (CanOpen, DS301Node, "Illegal PDO type given. Only RECEIVE_PDO and TRANSMIT_PDO are allowed." << endl);
00139   }
00140 
00141   // Now add the configured pdos to the node's mapping
00142   for (PDO::PDOStringMatchVec::iterator it = mapping.begin(); it != mapping.end(); ++it)
00143   {
00144     PDOMapEntry entry;
00145     entry.pdo_nr = pdo_nr;
00146     entry.pdo_mapping_index = it->pdo_mapping_index;
00147 
00148     map->insert(std::pair<std::string, PDOMapEntry>(it->name, entry));
00149   }
00150 }
00151 
00152 
00153 
00154 void DS301Node::uploadPDOs()
00155 {
00156   for (TPDO::PtrList::iterator it = m_tpdos.begin(); it != m_tpdos.end(); ++it)
00157   {
00158     (*it)->upload();
00159   }
00160 }
00161 
00162 void DS301Node::downloadPDOs()
00163 {
00164   for (RPDO::PtrList::iterator it = m_rpdos.begin(); it != m_rpdos.end(); ++it)
00165   {
00166     (*it)->download();
00167   }
00168 }
00169 
00170 void DS301Node::printPDOMapping()
00171 {
00172   uint32_t data32;
00173   size_t max_num_rpdo = 512;
00174   // get number of RPDOs
00175 
00176   std::stringstream ss;
00177   ss << "PDO Mapping queried from device:" << std::endl;
00178   ss << "===== RPDOs ===== " << std::endl;
00179 
00180   for (uint8_t i = 0; i < max_num_rpdo; ++i)
00181   {
00182     uint8_t num_entries;
00183     try
00184     {
00185       m_sdo.upload(false, 0x1600+i, 0, num_entries);
00186     }
00187     catch (const ProtocolException& e)
00188     {
00189       // This PDO object does not exist
00190       break;
00191     }
00192     ss << "  === RPDO " << static_cast<int>(i) << " - " << static_cast<int>(num_entries) << " entries ===" << std::endl;
00193 
00194     for (uint8_t j = 1; j <= num_entries; ++j)
00195     {
00196       m_sdo.upload(false, 0x1600+i, j, data32);
00197       int index = (data32 & 0xFFFF0000) >> 16;
00198       int subindex = (data32 & 0x0000FF00) >> 8;
00199       int length = data32 & 0x000000FF;
00200       ss << "    " << static_cast<int>(j) << " -> "
00201          << hexToString(index) << " / " << subindex << ", length: " << length << " bits" << std::endl;
00202     }
00203   }
00204 
00205   ss << std::endl;
00206 
00207   size_t max_num_tpdo = 512;
00208 
00209   ss << "===== TPDOs ===== " << std::endl;
00210 
00211   for (uint8_t i = 0; i < max_num_tpdo; ++i)
00212   {
00213     uint8_t num_entries;
00214     try
00215     {
00216       m_sdo.upload(false, 0x1A00+i, 0, num_entries);
00217     }
00218     catch (const ProtocolException& e)
00219     {
00220       // This PDO object does not exist
00221       break;
00222     }
00223     ss << "  === TPDO " << static_cast<int>(i) << " - " << static_cast<int>(num_entries) << " entries ===" << std::endl;
00224 
00225     for (uint8_t j = 1; j <= num_entries; ++j)
00226     {
00227       m_sdo.upload(false, 0x1A00+i, j, data32);
00228       int index = (data32 & 0xFFFF0000) >> 16;
00229       int subindex = (data32 & 0x0000FF00) >> 8;
00230       int length = data32 & 0x000000FF;
00231       ss << "    " << static_cast<int>(j) << " -> "
00232          << hexToString(index) << " / " << subindex << ", length: " << length << " bits" << std::endl;
00233     }
00234   }
00235 
00236   LOGGING_INFO (CanOpen, ss.str() << endl);
00237 }
00238 
00239 void DS301Node::registerPDONotifyCallback (const std::string& identifier,
00240                                            const boost::function< void() >& f)
00241 {
00242   boost::unordered_map<std::string,PDOMapEntry>::iterator it = m_tpdo_mapping.find(identifier);
00243   if(it != m_tpdo_mapping.end())
00244   {
00245      size_t pdo_nr = it->second.pdo_nr;
00246      m_tpdos[pdo_nr]->registerNotifyCallback(f);
00247      LOGGING_DEBUG_C (CanOpen, DS302Node, "Registered notification callback for PDO entry " <<
00248                             identifier << endl;
00249     );
00250   }
00251   else
00252   {
00253     std::stringstream ss;
00254     ss << "Notifier callback function for a PDO entry named " << identifier
00255        << " requested, however, no entry with this given identifier exists within this PDO";
00256     throw PDOException(ss.str());
00257   }
00258 }
00259 
00260 void DS301Node::stopNode()
00261 {
00262   m_nmt.stop();
00263 }
00264 
00265 
00266 
00267 }}// end of NS


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