NMT.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 //----------------------------------------------------------------------
00022 //----------------------------------------------------------------------
00023 
00024 #include "NMT.h"
00025 #include "Logging.h"
00026 
00027 namespace icl_hardware {
00028 namespace canopen_schunk {
00029 
00030 NMT::NMT(const uint8_t &node_id, const CanDevPtr &can_device):
00031   m_can_device(can_device),
00032   m_node_id(node_id),
00033   m_state(NMTS_INITIALISATION)  // All canopen devices start in this mode and send an NMT message once they switch to pre-operational
00034 {
00035 }
00036 
00037 void NMT::update(const CanMsg &msg)
00038 {
00039   uint8_t node_id = (msg.id - ds301::ID_NMT_ERROR_MIN)+1;
00040 
00041   if (node_id != m_node_id)
00042   {
00043     LOGGING_ERROR_C(CanOpen,NMT,"NMT Update called with wrong canopen ID. Received ID: " << node_id << " Node ID: " << m_node_id << ". Update ignored." << endl);
00044     return;
00045   }
00046 
00047   // NMT messages will ALWAYS be 1 byte long
00048   // Only exception: When we are the receiving end, someone could send commands to us. However the driver is not designed for that.
00049   if (msg.dlc != 1)
00050   {
00051     LOGGING_ERROR_C(CanOpen,NMT,"NMT Update called with illegal length message. Expected length: " << 1 << " got: " << msg.dlc << ". Update ignored." << endl);
00052     return;
00053   }
00054 
00055   uint8_t payload = msg.data[0];
00056 
00057   // The Bootup messages are always right
00058   if (payload == 0)
00059   {
00060     LOGGING_INFO_C(CanOpen,NMT,"NMT Bootup complete for node " << node_id  << endl);
00061     m_state = NMTS_PRE_OPERATIONAL;
00062   }
00063   else
00064   {
00065     // Here comes the handling of Heartbeat or nodeguard messages
00066     // Regardless of protocol we want to change our current state if it is not already the right one
00067     // We mask bit 1 as it is the toggle bit
00068     uint8_t state_to_check = (payload & 0x7F);
00069     if (isValidNmtState(state_to_check))
00070     {
00071       if (m_state != static_cast<eNMT_State>(state_to_check))
00072       {
00073         // HEARTBEAT OR NODEGUARDING FAIL!
00074         LOGGING_WARNING_C(CanOpen,NMT,"NMT Nodeguarding for node " << node_id << " detected a failure! State is supposed to be: " << nmtStateToString(m_state) << " but was detected as " << nmtStateToString(static_cast<eNMT_State>(state_to_check)) << ". State changed." << endl);
00075         m_state = static_cast<eNMT_State>(state_to_check);
00076       }
00077       else
00078       {
00079       // If the states match, everything is fine!
00080         LOGGING_TRACE_C(CanOpen,NMT,"NMT Nodeguarding for node " << node_id << " received. ALL IS WELL!");
00081       }
00082     }
00083     else
00084     {
00085       LOGGING_DEBUG_C(CanOpen,NMT,"NMT Nodeguarding for node " << node_id << " received illegal NMT state information. Ignoring message" << endl);
00086     }
00087   }
00088 
00089 }
00090 
00091 void NMT::start()
00092 {
00093  sendCommand(NMT_STARTREMOTENODE);
00094 }
00095 
00096 void NMT::stop()
00097 {
00098  sendCommand(NMT_STOPREMOTENODE);
00099 }
00100 
00101 void NMT::preOperational()
00102 {
00103  sendCommand(NMT_ENTERPREOPERATIONAL);
00104 }
00105 
00106 void NMT::reset()
00107 {
00108   sendCommand(NMT_RESETNODE);
00109 }
00110 
00111 void NMT::resetCommunication()
00112 {
00113   sendCommand(NMT_RESETCOMMUNICATION);
00114 }
00115 
00116 void NMT::sendCommand(const NMT::eNMT_Command& cmd)
00117 {
00118   // The transition 2 (initialization to Pre-Operational) is an automatic one and can not be forced
00119   // proactively set the internal state to the desired one.
00120   switch (cmd)
00121   {
00122     case  NMT_STARTREMOTENODE:  // Transition 3,6 or none
00123       m_state = NMTS_OPERATIONAL;
00124       break;
00125     case  NMT_STOPREMOTENODE:
00126       m_state = NMTS_STOPPED; // Transition 5,8 or none
00127       break;
00128     case  NMT_ENTERPREOPERATIONAL:
00129       m_state = NMTS_PRE_OPERATIONAL; //Transition 4,7 or none
00130       break;
00131     case  NMT_RESETNODE:
00132       m_state = NMTS_INITIALISATION;  // Transition 9,10,11 All values will be reset to turn on default, device reboots
00133       break;
00134     case  NMT_RESETCOMMUNICATION:
00135       m_state = NMTS_INITIALISATION;  // Transition 12,13,14 All communication values will be reset. Device has to finish bootup sequence again.
00136       break;
00137     default:
00138       LOGGING_ERROR_C(CanOpen,NMT,"Illegal NMT command " << cmd << " was sent to node with id " << m_node_id << " . Command ignored." << endl);
00139       return; // do not send anything
00140       break;
00141   }
00142 
00143   // Send the data straight forward
00144   // The NMT Commands are without response so no further actions have to be taken for the communication side.
00145   unsigned char msg[2];
00146   msg[0] = static_cast<unsigned char>(cmd);
00147   msg[1] = m_node_id;
00148   m_can_device->Send(CanMsg(ds301::ID_NMT,2,0,msg));
00149 }
00150 
00151 }}//end of NS


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