Added NMT reset callbacks, patch from Edward

[CanFestival-3.git] / src / nmtSlave.c

diff --git a/src/nmtSlave.c b/src/nmtSlave.c
index d088cb557862ed5d2d3f255f262121fd93011622..e9bce15609f1a3bf66cacc0a1b7d63b7b8da7b3c 100644 (file)
--- a/src/nmtSlave.c
+++ b/src/nmtSlave.c
@@ -33,6 +33,7 @@
 #include "nmtSlave.h"
 #include "states.h"
 #include "canfestival.h"
+#include "sysdep.h"
 
 /*!
 ** put the slave in the state wanted by the master
@@ -72,10 +73,22 @@ void proceedNMTstateChange(CO_Data* d, Message *m)
         break;
 
       case NMT_Reset_Node:
+         if(d->NMT_Slave_Node_Reset_Callback != NULL)
+            d->NMT_Slave_Node_Reset_Callback(d);
+#ifdef CO_ENABLE_LSS
+               if(getNodeId(d)!=d->lss_transfer.nodeID)
+                       setNodeId(d, d->lss_transfer.nodeID);
+#endif
         setState(d,Initialisation);
         break;
 
       case NMT_Reset_Comunication:
+         if(d->NMT_Slave_Communications_Reset_Callback != NULL)
+            d->NMT_Slave_Communications_Reset_Callback(d);
+#ifdef CO_ENABLE_LSS
+               if(getNodeId(d)!=d->lss_transfer.nodeID && getNodeId(d)>0 && getNodeId(d)<=127)
+                       setNodeId(d, d->lss_transfer.nodeID);
+#endif
         setState(d,Initialisation);
         break;
 
@@ -105,7 +118,10 @@ UNS8 slaveSendBootUp(CO_Data* d)
   MSG_WAR(0x3407, "Send a Boot-Up msg ", 0);
 
   /* message configuration */
-  m.cob_id.w = NODE_GUARD << 7 | *d->bDeviceNodeId;
+  {
+         UNS16 tmp = NODE_GUARD << 7 | *d->bDeviceNodeId; 
+         m.cob_id = UNS16_LE(tmp);
+  }
   m.rtr = NOT_A_REQUEST;
   m.len = 1;
   m.data[0] = 0x00;