diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
index d819e1d..b095076 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
@@ -25,6 +25,7 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
                                      uint32_t nodeId,  // 0x580 + CobId
                                      int exeSampleTimeMs,
                                      const char* name,
+                                     int heartTimeoutMs,
                                      int dbgMode) {
 
   writeBuffer_        = writeBuffer;
@@ -32,12 +33,14 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
   exeSampleTimeMs_    = exeSampleTimeMs;
   exeCounter_         = 0;
   errorCode_          = 0;
+  heartTimeoutMs_     = heartTimeoutMs;
   dbgMode_            = dbgMode;
   name_               = strdup(name);
   isMaster_           = false;  
   nmtState_           = NMT_NOT_VALID;
   nmtStateOld_        = NMT_NOT_VALID;
   nmtActParam_        = NULL;
+  heartBeatCounter_   = 0;
   pdoCounter_ = 0;
   sdoCounter_ = 0;
   sdo1Lock_.test_and_set();   // make sure only one sdo is accessing the bus at the same time
@@ -64,8 +67,7 @@ ecmcCANOpenDevice::~ecmcCANOpenDevice() {
 
 void ecmcCANOpenDevice::execute() {
   
-  exeCounter_++;
-
+  exeCounter_++;  
   for(int i=0 ; i<pdoCounter_; i++) {
     if(pdos_[i]) {
       pdos_[i]->execute();
@@ -77,6 +79,16 @@ void ecmcCANOpenDevice::execute() {
       sdos_[i]->execute();
     }
   }
+  
+  // NMT hearbeat timout
+  if (heartBeatCounter_ * exeSampleTimeMs_ >= heartTimeoutMs_) {
+    nmtStateOld_ = nmtState_;
+    nmtState_ = NMT_NOT_VALID;
+    nmtActParam_->refreshParam(1);
+  }
+  else {
+    heartBeatCounter_ ++;
+  }
 
   return;
 }
@@ -125,6 +137,7 @@ int ecmcCANOpenDevice::validateFrame(can_frame *frame) {
 }
 
 int ecmcCANOpenDevice::checkNMT(can_frame *frame) {
+
   // check if NMT frame
   if(frame->can_id == (ECMC_CANOPEN_NMT_BASE + nodeId_)) {
     if(frame->can_dlc == 1){
@@ -146,11 +159,13 @@ int ecmcCANOpenDevice::checkNMT(can_frame *frame) {
           break;
       }
     }
+    heartBeatCounter_ = 0;
     if(nmtState_ != nmtStateOld_) {
       nmtActParam_->refreshParam(1);
     }
-    nmtState_ = nmtStateOld_;
+    nmtStateOld_ = nmtState_;    
   }
+
   return 0;
 }
 
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
index affb187..1eaebe6 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
@@ -41,6 +41,7 @@ class ecmcCANOpenDevice {
                     uint32_t nodeId,
                     int exeSampleTimeMs,
                     const char* name,
+                    int heartTimeoutMs,
                     int dbgMode);
   virtual ~ecmcCANOpenDevice();
   void execute();
@@ -74,6 +75,8 @@ class ecmcCANOpenDevice {
   int pdoCounter_;
   int sdoCounter_;
   char* name_;
+  int heartTimeoutMs_;
+  int heartBeatCounter_;
   ecmcCANOpenPDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT];
   ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT];
   bool isMaster_;
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp
index 344457d..b553d16 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp
@@ -31,6 +31,7 @@ ecmcCANOpenMaster::ecmcCANOpenMaster(ecmcSocketCANWriteBuffer* writeBuffer,
                                      nodeId,  // 0x580 + CobId
                                      exeSampleTimeMs,
                                      name,
+                                     0,  //NMT timeout is 0 for master (always OP)
                                      dbgMode) {
   lssPdo_            = NULL;
   syncPdo_           = NULL;
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
index b676c41..aa06313 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
@@ -336,7 +336,8 @@ void ecmcSocketCAN::addMaster(uint32_t nodeId,
 }
 
 void ecmcSocketCAN::addDevice(uint32_t nodeId,
-                              const char* name){
+                              const char* name,
+                              int heartTimeoutMs){
   if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
     throw std::out_of_range("Device array full.");
   }
@@ -344,7 +345,7 @@ void ecmcSocketCAN::addDevice(uint32_t nodeId,
     throw std::out_of_range("Node id out of range.");
   }
 
-  devices_[deviceCounter_] = new ecmcCANOpenDevice(writeBuffer_,nodeId,exeSampleTimeMs_,name, cfgDbgMode_);  
+  devices_[deviceCounter_] = new ecmcCANOpenDevice(writeBuffer_,nodeId,exeSampleTimeMs_,name,heartTimeoutMs,cfgDbgMode_);  
   deviceCounter_++;
 }
 
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
index 28bd14b..38a607a 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
@@ -89,7 +89,8 @@ class ecmcSocketCAN {
                                   int heartSampleTimeMs);
 
   void                  addDevice(uint32_t nodeId,
-                                  const char* name);
+                                  const char* name,
+                                  int heartTimeoutMs);
 
   void                  addPDO(uint32_t nodeId,
                                uint32_t cobId,
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
index cd4bf09..c17f05c 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
@@ -153,11 +153,11 @@ void deleteSocketCAN() {
 void ecmcCANOpenAddMasterPrintHelp() {
   printf("\n");
   printf("       Use ecmcCANOpenAddMaster(<name>, <node id>,....)\n");
-  printf("          <name>                     : Name of master device.\n");
-  printf("          <node id>                  : CANOpen node id of master.\n");
-  printf("          <LSS sample time ms>       : Sample time for LSS.\n");
-  printf("          <Sync sample time ms>      : Sample time for SYNC.\n");
-  printf("          <Heartbeat sample time ms> : Sample time for Heartbeat.\n");
+  printf("          <name>                         : Name of master device.\n");
+  printf("          <node id>                      : CANOpen node id of master.\n");
+  printf("          <LSS sample time ms>           : Sample time for LSS.\n");
+  printf("          <Sync sample time ms>          : Sample time for SYNC.\n");
+  printf("          <NMT Heartbeat sample time ms> : Sample time for NMT Heartbeat.\n");
   printf("\n");
 }
 
@@ -207,7 +207,7 @@ static const iocshArg initArg2_0 =
 static const iocshArg initArg3_0 =
 { "Sync sample time ms", iocshArgInt };
 static const iocshArg initArg4_0 =
-{ "Heart sample time ms", iocshArgInt };
+{ "NMT Heart sample time ms", iocshArgInt };
 
 static const iocshArg *const initArgs_0[]  = { &initArg0_0, 
                                                &initArg1_0,
@@ -231,12 +231,13 @@ static void initCallFunc_0(const iocshArgBuf *args) {
 void ecmcCANOpenAddDevicePrintHelp() {
   printf("\n");
   printf("       Use ecmcCANOpenAddDevice(<name>, <node id>)\n");
-  printf("          <name>      : Name of device.\n");
-  printf("          <node id>   : CANOpen node id of device.\n");
+  printf("          <name>                     : Name of device.\n");
+  printf("          <node id>                  : CANOpen node id of device.\n");
+  printf("          <NMT Heartbeat timeout ms> : Timeout for NMT Heartbeat.\n");
   printf("\n");
 }
 
-int ecmcCANOpenAddDevice(const char* name, int nodeId) {
+int ecmcCANOpenAddDevice(const char* name, int nodeId,int heartTimeOutMs) {
   if(!name) {
     printf("Error: name.\n");
     ecmcCANOpenAddDevicePrintHelp();
@@ -253,8 +254,13 @@ int ecmcCANOpenAddDevice(const char* name, int nodeId) {
     return asynError;
   }
 
+  if(heartTimeOutMs < 0) {
+    printf("Invalid NMT heartbeat timeout.\n");
+    return asynError;
+  }
+
   try {
-    can->addDevice((uint32_t)nodeId,name);
+    can->addDevice((uint32_t)nodeId,name,heartTimeOutMs);
   }
   catch(std::exception& e) {
     printf("Exception: %s. Add device failed.\n",e.what());
@@ -268,13 +274,16 @@ static const iocshArg initArg0_1 =
 { "Name", iocshArgString };
 static const iocshArg initArg1_1 =
 { "Node Id", iocshArgInt };
+static const iocshArg initArg2_1 =
+{ "NMT Heart timeout ms", iocshArgInt };
 
 static const iocshArg *const initArgs_1[]  = { &initArg0_1, 
-                                               &initArg1_1};
+                                               &initArg1_1,
+                                               &initArg2_1};
 
-static const iocshFuncDef    initFuncDef_1 = { "ecmcCANOpenAddDevice", 2, initArgs_1 };
+static const iocshFuncDef    initFuncDef_1 = { "ecmcCANOpenAddDevice", 3, initArgs_1 };
 static void initCallFunc_1(const iocshArgBuf *args) {
-  ecmcCANOpenAddDevice(args[0].sval, args[1].ival);
+  ecmcCANOpenAddDevice(args[0].sval, args[1].ival, args[2].ival);
 }
 
 /**